#include "utility.h"

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>

#include <gtsam/nonlinear/ISAM2.h>
#include <pcl/filters/crop_box.h>  //edit
#include <pcl/common/transforms.h>  //edit

#include <gflags/gflags.h>//edit  //from "map_builder_node.cc" 
#include <glog/logging.h>

#include <pcl/io/pcd_io.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>

#include <thread>

#include <geometry_msgs/Quaternion.h>

#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include "utils/TicToc.h"
#include "utils/common_ros.h"

// #include "loam/srv_record_map.h"  //edit loop

#include "factor/lidarFactor.hpp"
#include "factor/AutoPoseLocalParameterization.h"
#include "factor/PoseLocalParameterization.h"
#include "utils/CircularBuffer.h"
#include "point_processor/PointMapping.h"

#include <unordered_map>

using namespace gtsam;

namespace lio {
    
typedef Twist<float> Transform;
using namespace mathutils;

class mapOptimization : public ParamServer {

private:

    NonlinearFactorGraph gtSAMgraph;
    Values initialEstimate;
    Values optimizedEstimate;
    ISAM2 *isam;
    Values isamCurrentEstimate;

    noiseModel::Diagonal::shared_ptr priorNoise;
    noiseModel::Diagonal::shared_ptr odometryNoise;
    noiseModel::Diagonal::shared_ptr constraintNoise;

    ros::Publisher pubLaserCloudSurround;
    ros::Publisher pubOdomAftMapped;
    ros::Publisher pubKeyPoses;

    ros::Publisher pubHistoryKeyFrames;
    ros::Publisher pubIcpKeyFrames;
    ros::Publisher pubRecentKeyFrames;
	ros::Publisher pubLaserCloudmap;  //edit

    ros::Subscriber subLaserCloudCornerLast;
    ros::Subscriber subLaserCloudSurfLast;
    ros::Subscriber subOutlierCloudLast;
    ros::Subscriber subLaserOdometry;
    ros::Subscriber subImu;
	
	ros::ServiceServer srv_record_map;  //edit loop

    nav_msgs::Odometry odomAftMapped;
    tf::StampedTransform aftMappedTrans;
    tf::TransformBroadcaster tfBroadcaster;

    vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
    vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
    vector<pcl::PointCloud<PointType>::Ptr> outlierCloudKeyFrames;

    deque<pcl::PointCloud<PointType>::Ptr> recentCornerCloudKeyFrames;
    deque<pcl::PointCloud<PointType>::Ptr> recentSurfCloudKeyFrames;
    deque<pcl::PointCloud<PointType>::Ptr> recentOutlierCloudKeyFrames;
    int latestFrameID;

    vector<int> surroundingExistingKeyPosesID;
    deque<pcl::PointCloud<PointType>::Ptr> surroundingCornerCloudKeyFrames;
    deque<pcl::PointCloud<PointType>::Ptr> surroundingSurfCloudKeyFrames;
    deque<pcl::PointCloud<PointType>::Ptr> surroundingOutlierCloudKeyFrames;
    
    PointType previousRobotPosPoint;
    PointType currentRobotPosPoint;

    pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
    pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;

    pcl::PointCloud<PointType>::Ptr laserCloudCornerLast;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLast;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS;

    pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDSformap;  //edit downsize 2nd
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDSformap;  //edit downsize 2nd
    pcl::PointCloud<PointType>::Ptr laserCloudOutlierLastDSformap;  //edit downsize 2nd

    pcl::PointCloud<PointType>::Ptr laserCloudOutlierLast;
    pcl::PointCloud<PointType>::Ptr laserCloudOutlierLastDS;
	pcl::PointCloud<PointType>::Ptr laserCloudOutlierLastIntensity;	  //反射强度精简

    pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLast;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLastDS;

    pcl::PointCloud<PointType>::Ptr laserCloudOri;
    pcl::PointCloud<PointType>::Ptr coeffSel;

    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;

    pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;  //注意：lio的程序用的new的
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;

    pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap1;  //edit
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap1;

    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;

    
    pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloudDS;
    pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloudDS;

    pcl::PointCloud<PointType>::Ptr latestCornerKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloudDS;

    pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap;
	pcl::PointCloud<PointType>::Ptr globalMap; //edit

    std::vector<int> pointSearchInd;
    std::vector<float> pointSearchSqDis;

    pcl::VoxelGrid<PointType> downSizeFilterCorner;
    pcl::VoxelGrid<PointType> downSizeFilterSurf;
    pcl::VoxelGrid<PointType> downSizeFilterOutlier;
    pcl::VoxelGrid<PointType> downSizeFilterICP;

    pcl::VoxelGrid<PointType> downSizeFilterCornerformap;  //edit downsize 2nd
    pcl::VoxelGrid<PointType> downSizeFilterSurfformap;  //edit downsize 2nd
    pcl::VoxelGrid<PointType> downSizeFilterOutlierformap;  //edit downsize 2nd

    pcl::VoxelGrid<PointType> downSizeFilterHistoryKeyFrames;
    pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses;
    pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses;
    pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames;

    double timeLaserCloudCornerLast;
    double timeLaserCloudSurfLast;
    double timeLaserOdometry;
    double timeLaserCloudOutlierLast;
    double timeLastGloalMapPublish;

    bool newLaserCloudCornerLast;
    bool newLaserCloudSurfLast;
    bool newLaserOdometry;
    bool newLaserCloudOutlierLast;
	
	bool is_record_map = false;  //edit loop
	bool is_record_pose = false;
	bool yes_record_pose = false;
	string point_bfile;//平面点文件名
	string Poses6D_bfile;//边缘点文件名
	int Num_Points = 0;
	int Num_Poses = 0;
	int zeroPCount = 11;
	int saveKCount = 0;

	int pbtest = 0;

	struct VMapPoint{  //edit loop
		float x;
		float y;
		float z;
		float intensity;  //反射强度修改
	};  
	
	struct VPoses6D{  //edit loop
		float x;
		float y;
		float z;
		float roll;
		float pitch;
		float yaw;
	};

	int convergence = 0;
	
	int mapNum = 0;

	ofstream fout_point;

	bool zone1 = false;
	bool zone2 = false;
	bool zone3 = false;
	bool zone4 = false;
	bool zone5 = false;
	bool zone6 = false;

	int Num_rotation = 0;
	bool flag_rotation = false;

	int filter_rot_start = 0;

    // float transformLast[6];
    // float transformSum[6];
    // float transformIncre[6];
    // float transformTobeMapped[6];
    // float transformBefMapped[6];
    // float transformAftMapped[6];
    Transform transform_gtsam_last_; //在gtsam中使用，与3帧中最后一帧位姿进行区分
    Transform transform_aft_mapped_;
    Transform transform_Tobe_Mapped_NoCon_;

    Transform transform_odom_;
    Transform transform_align_;
    Transform transform_AftAftMapped_;

    Transform transform_sum_;  //edit
    Transform transform_tobe_mapped_;
    Transform transform_bef_mapped_;
    Eigen::Matrix<float, 6, 6> matP_;
    multimap<float, pair<PointT, PointT>, greater<float> > score_point_coeff_;
    


    // float transformTobeMappedNoCon[6];  //edit filter no convergence

    // float transformodom[6];
	// float transformalign[6];
    // float transformAftAftMapped[6];


    int imuPointerFront;
    int imuPointerLast;

    std::mutex mtx;

    double timeLastProcessing;

    PointType pointOri, pointSel, pointProj, coeff;  //pcl::PointXYZI

    cv::Mat matA0;
    cv::Mat matB0;
    cv::Mat matX0;

    cv::Mat matA1;
    cv::Mat matD1;
    cv::Mat matV1;

    bool isDegenerate;
    cv::Mat matP;

    int laserCloudCornerFromMapDSNum;
    int laserCloudSurfFromMapDSNum;
    int laserCloudCornerLastDSNum;
    int laserCloudSurfLastDSNum;
    int laserCloudOutlierLastDSNum;
    int laserCloudSurfTotalLastDSNum;

    bool potentialLoopFlag;
    double timeSaveFirstCurrentScanForLoopClosure;
    int closestHistoryFrameID;
    int latestFrameIDLoopCloure;

    bool aLoopIsClosed;

    float cRoll, sRoll, cPitch, sPitch, cYaw, sYaw, tX, tY, tZ;
    float ctRoll, stRoll, ctPitch, stPitch, ctYaw, stYaw, tInX, tInY, tInZ;

    // double poseparam[6] = {0, 0, 0, 0, 0, 0};  //用于rpy+平移的优化
    double lioposeparam[7] = {0, 0, 0, 1, 0, 0, 0};
    
    bool system_init_ = false;  
    // 三帧优化
    // const size_t frame_size = 3;    //一起优化的帧数
    const int skip_frames = 25;   //最开始的skip_frames帧不使用多帧优化
    bool flag_normal_ = true; //为1,表示单帧，0表示多帧
    // bool flag_4Dconstrain_ = 0;  //为1，表示使用旋转约束；为0，表示不用旋转约束
    double **para_pose_;  //用于多帧优化的参数
    Transform transform_last_;  //用于记录n帧中最后一帧的位姿
    int skip_count_ = 2;  //edit
    int odom_count_ = 0;

    CircularBuffer<Transform> all_laser_transforms_{frame_size};
    CircularBuffer<Transform> transform_sums_{frame_size}; //存estimator发来的位姿
    CircularBuffer<pcl::PointCloud<PointT>::Ptr> surf_stack_{frame_size};
    CircularBuffer<pcl::PointCloud<PointT>::Ptr> corner_stack_{frame_size};
    CircularBuffer<pcl::PointCloud<PointT>::Ptr> full_stack_{frame_size};
    CircularBuffer<double> laser_odom_times_{frame_size};

    ros::Subscriber sub_map_compact_data_;
    mutex buf_mutex_;
    std::condition_variable con_;
    queue<sensor_msgs::PointCloud2ConstPtr> compact_data_buf_;  //消息
    queue<pcl::PointCloud<PointType>::Ptr> compact_corner_clouds_;    //pcl点云
    queue<pcl::PointCloud<PointType>::Ptr> compact_surf_clouds_;
    queue<pcl::PointCloud<PointType>::Ptr> compact_full_clouds_;
    queue<Transform> compact_transform_sums_;
    queue<ros::Time> compact_timestamps_;
    // end

    std::map<int, std::pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
    ros::Publisher pubPath;
    nav_msgs::Path globalPath;
    ros::Publisher pubLoopConstraintEdge;
    ros::Subscriber subLoop;
    std::mutex mtxLoopInfo;
    std::vector<std::pair<int, int>> loopIndexQueue;
    std::vector<gtsam::Pose3> loopPoseQueue;
    std::vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;
    deque<std_msgs::Float64MultiArray> loopInfoVec;
    pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;
    pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;
    std::map<int, int> loopIndexContainer; // from new to old
    
    std::shared_ptr<LidarIris> lidarIris;
    pcl::PointCloud<PointType>::Ptr featurePoses;  // 用于计算iris特征的帧的位置
    pcl::PointCloud<PointType>::Ptr copy_featurePoses;  // 用于计算iris特征的帧的位置

    std::unordered_map<int, std::string> fullCloudHash;
    pcl::PointCloud<PointType>::Ptr fullCloudAlignWorld; 

public:

    

    mapOptimization()
    {
    	ISAM2Params parameters;
		parameters.relinearizeThreshold = 0.01;
		parameters.relinearizeSkip = 1;
    	isam = new ISAM2(parameters);

        //pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("/key_pose_origin", 2);  //edit to loam
        pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/mapping/laser_cloud_surround", 2);  //edit to loam
        pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/mapping/final_aft_mapped_to_init", 5);  //edit
	    pubLaserCloudmap = nh.advertise<sensor_msgs::PointCloud2>("/mapping/final_cloud_registered", 2);  //edit

        pubPath = nh.advertise<nav_msgs::Path>("/mapping/path", 1);
        pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/mapping/loop_closure_constraints", 1);

        subLoop  = nh.subscribe<std_msgs::Float64MultiArray>("/lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());

        sub_map_compact_data_ = nh.subscribe<sensor_msgs::PointCloud2>
                                ("/map_compact_data", 2, &mapOptimization::MapCompactHandler, this);

        downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
        downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity);

        odomAftMapped.header.frame_id = "world";
        odomAftMapped.child_frame_id = "aft_mapped";

        aftMappedTrans.frame_id_ = "world";
        aftMappedTrans.child_frame_id_ = "aft_mapped";

        allocateMemory();

        lidarIris = std::make_shared<LidarIris>(4, 18, 1.6, 0.75, 50);
        featurePoses.reset(new pcl::PointCloud<PointType>());
        copy_featurePoses.reset(new pcl::PointCloud<PointType>());
    }

    ~mapOptimization() {
        for (int i = 0; i < frame_size; ++i) {
            delete[] para_pose_[i];
        }        
        delete[] para_pose_;
    }

    void allocateMemory(){

        cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
        cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
        copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
        copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());

        // surroundingKeyPoses.reset(new pcl::PointCloud<PointType>());
        // surroundingKeyPosesDS.reset(new pcl::PointCloud<PointType>());

        kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
        kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());      

        laserCloudCornerLast.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());
        laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>());

        laserCloudCornerLastDSformap.reset(new pcl::PointCloud<PointType>());  //edit downsize 2nd
        laserCloudSurfLastDSformap.reset(new pcl::PointCloud<PointType>());  //edit downsize 2nd
        laserCloudOutlierLastDSformap.reset(new pcl::PointCloud<PointType>());  //edit downsize 2nd

        laserCloudOutlierLast.reset(new pcl::PointCloud<PointType>());
		laserCloudOutlierLastIntensity.reset(new pcl::PointCloud<PointType>());  //反射强度精简
        laserCloudOutlierLastDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfTotalLast.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfTotalLastDS.reset(new pcl::PointCloud<PointType>());

        laserCloudOri.reset(new pcl::PointCloud<PointType>());
        coeffSel.reset(new pcl::PointCloud<PointType>());

        laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());

        kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
        kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());

        kdtreeCornerFromMap1.reset(new pcl::KdTreeFLANN<PointType>());  //edit
        kdtreeSurfFromMap1.reset(new pcl::KdTreeFLANN<PointType>());
        
        nearHistoryCornerKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
        nearHistoryCornerKeyFrameCloudDS.reset(new pcl::PointCloud<PointType>());
        nearHistorySurfKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
        nearHistorySurfKeyFrameCloudDS.reset(new pcl::PointCloud<PointType>());

        latestCornerKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
        latestSurfKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
        latestSurfKeyFrameCloudDS.reset(new pcl::PointCloud<PointType>());

        kdtreeGlobalMap.reset(new pcl::KdTreeFLANN<PointType>());
		globalMap.reset(new pcl::PointCloud<PointType>()); //edit

        fullCloudAlignWorld.reset(new pcl::PointCloud<PointType>());

        timeLaserCloudCornerLast = 0;
        timeLaserCloudSurfLast = 0;
        timeLaserOdometry = 0;
        timeLaserCloudOutlierLast = 0;
        timeLastGloalMapPublish = 0;

        timeLastProcessing = -1;

        newLaserCloudCornerLast = false;
        newLaserCloudSurfLast = false;

        newLaserOdometry = false;
        newLaserCloudOutlierLast = false;

        gtsam::Vector Vector6(6);
        Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6;
        priorNoise = noiseModel::Diagonal::Variances(Vector6);
        odometryNoise = noiseModel::Diagonal::Variances(Vector6);

        matA0 = cv::Mat (5, 3, CV_32F, cv::Scalar::all(0));
        matB0 = cv::Mat (5, 1, CV_32F, cv::Scalar::all(-1));
        matX0 = cv::Mat (3, 1, CV_32F, cv::Scalar::all(0));

        matA1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0));
        matD1 = cv::Mat (1, 3, CV_32F, cv::Scalar::all(0));
        matV1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0));

        isDegenerate = false;
        matP = cv::Mat (6, 6, CV_32F, cv::Scalar::all(0));

        laserCloudCornerFromMapDSNum = 0;
        laserCloudSurfFromMapDSNum = 0;
        laserCloudCornerLastDSNum = 0;
        laserCloudSurfLastDSNum = 0;
        laserCloudOutlierLastDSNum = 0;
        laserCloudSurfTotalLastDSNum = 0;

        potentialLoopFlag = false;
        aLoopIsClosed = false;

        latestFrameID = 0;

        //  多帧优化初始化
        para_pose_ = new double *[frame_size];
        for (int i = 0; i < frame_size; ++i) {
            para_pose_[i] = new double[7];
        }
        for (size_t i = 0; i < frame_size; ++i) {
            all_laser_transforms_[i].pos.setZero();
            all_laser_transforms_[i].rot.setIdentity();
            laser_odom_times_[i] = 0.;

            surf_stack_[i].reset();
            corner_stack_[i].reset();
            full_stack_[i].reset();
        }
    }
	
	void save_pointsize_for_map(int size)  //edit loop
	{
		ofstream fout(point_bfile.c_str(), ios::app | ios ::binary | ios::out);
		if(!fout.is_open())
			return;
        
		int tmpsize;
		tmpsize = size;

		fout.write((char*)&tmpsize ,sizeof(int));
		fout.close();
	}
	
	void save_point_for_map(pcl::PointXYZI data)  //edit loop  //反射强度修改
	{
		//ofstream fout(point_bfile.c_str(), ios::app | ios ::binary | ios::out);
		//if(!fout.is_open())
			//return;
        
		VMapPoint tmp;
		tmp.x = data.x;
		tmp.y = data.y;
		tmp.z = data.z; 
		tmp.intensity = data.intensity;  //反射强度修改

		fout_point.write((char*)&tmp ,sizeof(VMapPoint));
		//fout.close();
	}

	void save_Poses6D_for_map(PointTypePose Poses6D)  //edit loop    ???? : pcl::PointXYZIRPYT
	{
		ofstream fout(Poses6D_bfile.c_str(), ios::app | ios ::binary | ios::out);
		if(!fout.is_open())
			return;
        
		VPoses6D tmpPose;
		tmpPose.x = Poses6D.x;
		tmpPose.y = Poses6D.y;
		tmpPose.z = Poses6D.z; 
        Eigen::Quaternionf q = Eigen::Quaternionf(Poses6D.qw, Poses6D.qx, Poses6D.qy, Poses6D.qz);
        Eigen::Vector3f euler = q.normalized().toRotationMatrix().eulerAngles(2, 1, 0);//得到World to Lidar欧拉角(z, y, x)
        Eigen::Vector3f thiseuler(euler.z(), euler.y(), euler.x());
		tmpPose.roll = thiseuler.x();
		tmpPose.pitch = thiseuler.y();
		tmpPose.yaw = thiseuler.z(); 

		fout.write((char*)&tmpPose ,sizeof(VPoses6D));
		fout.close();
	}	
	
	//服务回调函数，是否存储特征点map
	// bool recordMapHandler(loam::srv_record_map::Request &req,  //edit loop
	// 					loam::srv_record_map::Response &res)
	// {
	// 	point_bfile.clear();
	// 	Poses6D_bfile.clear();
		
	// 		if(!req.is_record_pose)    //???? or another srv
	// 		{
	// 			if(req.is_record_map)
	// 			{
	// 				is_record_map = true;
	// 				point_bfile = "/home/robot/loam_ws/map/" + req.str_map_name + "_points.map";
	// 				res.feedback = "Start to save points!";  
					
	// 				return true;
	// 			}
	// 			else
	// 			{
	// 				is_record_map = false;
	// 				res.feedback = "Stop to save points!";  
					
	// 				return false;
	// 			}
	// 		}
		
	// 		if(req.is_record_pose)
	// 		{
	// 			is_record_pose = true;
	// 			Poses6D_bfile = "/home/robot/loam_ws/map/" + req.str_map_name + "_Poses6D.map";
	// 			res.feedback = "Start to save pose6D!"; 
				
	// 			if(yes_record_pose)  //已经record完成
	// 			{
	// 				return true;
	// 			}
	// 			else
	// 			{
	// 				return false;
	// 			}
	// 		}
	// 		else
	// 		{
	// 			is_record_pose = false;
	// 			res.feedback = "Stop to save pose6D!";  
	// 			return false;
	// 		}
	// }
	
	template<typename Derived>  //edit
	inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
	ConstrainedRotAxis(const Eigen::QuaternionBase<Derived> &quat_in) {  //旋转约束，不知道什么玩意
	  typedef typename Derived::Scalar Scalar;
	  Eigen::Quaternion<Scalar> q_R = quat_in.normalized();
	  Eigen::AngleAxis<Scalar> angle_axis_R(q_R);
	  Eigen::Matrix<Scalar, 3, 1> axis_R = angle_axis_R.axis();
	  Scalar angle_R = angle_axis_R.angle();
	  Scalar cot_half_angle = (isinf(1 / tan(angle_R / 2)) ? 1e6 : (1 / tan(angle_R / 2)));
	  Eigen::Matrix<Scalar, 3, 1> delta_axis_R{-axis_R.y(), axis_R.x(), cot_half_angle};
	  Eigen::Matrix<Scalar, 3, 1> constrained_axis = axis_R.cross(delta_axis_R);
	  constrained_axis.normalize();
	  return constrained_axis;
	}
	
	void Transform4DAssociateToMap() {
        Transform transform_incre(transform_bef_mapped_.inverse() * transform_sum_.transform());
        Transform full_transform = transform_tobe_mapped_ * transform_incre;

        Eigen::Vector3d origin_R0 = R2ypr(full_transform.rot.normalized().toRotationMatrix().cast<double>());

        Eigen::Vector3d origin_R00 = R2ypr(transform_sum_.rot.normalized().toRotationMatrix().cast<double>());

        // Z-axix R00 to R0, regard para_pose's R as rotate along the Z-axis first
        double y_diff = origin_R0.x() - origin_R00.x();

        //TODO
        Eigen::Matrix3f rot_diff = ypr2R(Eigen::Vector3f(y_diff, 0, 0));

        transform_tobe_mapped_.pos = full_transform.pos;
        transform_tobe_mapped_.rot = rot_diff * transform_sum_.rot.normalized();
    }

    void transformAssociateToMap()
    {
        Transform transform_incre(transform_bef_mapped_.inverse() * transform_sum_.transform());
        transform_tobe_mapped_ = transform_tobe_mapped_ * transform_incre;
    }

    void transformUpdate()    //edit
    {
		transform_bef_mapped_ = transform_sum_;
        transform_aft_mapped_ = transform_tobe_mapped_;
    }

    void MultiTransform4DAssociateToMap() {  //多帧优化用到      
        // transform_bef_mapped_存的是前一批中最后一个transform_sum_.
        // transform_last_存的是上一批n帧点云中最后一帧的位姿
        // 这里的transform_tobe_mapped_只当做存储预估值的中间变量
        Transform transform_incre(transform_bef_mapped_.inverse() * transform_sum_.transform());
        Transform full_transform = transform_last_ * transform_incre; 

        Eigen::Vector3d origin_R0 = R2ypr(full_transform.rot.normalized().toRotationMatrix().cast<double>());

        Eigen::Vector3d origin_R00 = R2ypr(transform_sum_.rot.normalized().toRotationMatrix().cast<double>());

        // Z-axix R00 to R0, regard para_pose's R as rotate along the Z-axis first
        double y_diff = origin_R0.x() - origin_R00.x();

        //TODO
        Eigen::Matrix3f rot_diff = ypr2R(Eigen::Vector3f(y_diff, 0, 0));

        transform_tobe_mapped_.pos = full_transform.pos;
        transform_tobe_mapped_.rot = rot_diff * transform_sum_.rot.normalized();	
	}

    void MultitransformAssociateToMap() {  //多帧优化用到
        // transform_bef_mapped_存的是前一批中最后一个transform_sum_.
        // transform_last_存的是上一批n帧点云中最后一帧的位姿
        // 这里的transform_tobe_mapped_只当做存储预估值的中间变量
        Transform transform_incre(transform_bef_mapped_.inverse() * transform_sum_.transform());
        transform_tobe_mapped_ = transform_last_ * transform_incre; 	
	}

	void pointAssociateToMap(const pcl::PointXYZI &pi, pcl::PointXYZI &po, const Transform &transform_tobe_mapped)    //edit  反射强度修改
	{
        po.x = pi.x;
        po.y = pi.y;
        po.z = pi.z;
        po.intensity = pi.intensity;

        RotatePoint(transform_tobe_mapped.rot, po);

        po.x += transform_tobe_mapped.pos.x();
        po.y += transform_tobe_mapped.pos.y();
        po.z += transform_tobe_mapped.pos.z();        
	}

    pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn){
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
        PointType *pointFrom;
        PointType pointTo;
        Eigen::Quaternionf q;
        q = Eigen::Quaternionf(transformIn->qw, transformIn->qx, transformIn->qy, transformIn->qz);

        int cloudSize = cloudIn->points.size();
        cloudOut->resize(cloudSize);
        
        for (int i = 0; i < cloudSize; ++i){

            pointFrom = &cloudIn->points[i]; 
            Eigen::Vector3f vecFrom, vec;
            vecFrom.x() = pointFrom->x;  
            vecFrom.y() = pointFrom->y;
            vecFrom.z() = pointFrom->z;   
            vec = q * vecFrom;
            pointTo.x = vec.x() + transformIn->x;
            pointTo.y = vec.y() + transformIn->y;
            pointTo.z = vec.z() + transformIn->z;
            pointTo.intensity = pointFrom->intensity;

            cloudOut->points[i] = pointTo;
        }
        return cloudOut;
    }

    pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, Transform &transformIn){
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
        PointType *pointFrom;
        PointType pointTo;
        Eigen::Quaternionf q;
        q = transformIn.rot;
        Eigen::Vector3f tranlation = transformIn.pos;

        int cloudSize = cloudIn->points.size();
        cloudOut->resize(cloudSize);
        
        for (int i = 0; i < cloudSize; ++i){

            pointFrom = &cloudIn->points[i]; 
            Eigen::Vector3f vecFrom, vec;
            vecFrom.x() = pointFrom->x;  
            vecFrom.y() = pointFrom->y;
            vecFrom.z() = pointFrom->z;   
            vec = q * vecFrom;
            pointTo.x = vec.x() + tranlation.x();
            pointTo.y = vec.y() + tranlation.y();
            pointTo.z = vec.z() + tranlation.z();
            pointTo.intensity = pointFrom->intensity;

            cloudOut->points[i] = pointTo;
        }
        return cloudOut;
    }

    void MapCompactHandler(const sensor_msgs::PointCloud2ConstPtr &compact_data_msg) {
        buf_mutex_.lock();
        // LOG(INFO) << "GET COMPACT";
        compact_data_buf_.push(compact_data_msg);
        buf_mutex_.unlock();
        con_.notify_one();
    }

    void CompactDataHandler(const sensor_msgs::PointCloud2ConstPtr &compact_data_msg) {

        TicToc tic_toc_decoder;

        PointCloud compact_points;
        pcl::fromROSMsg(*compact_data_msg, compact_points);

        size_t compact_point_size = compact_points.size();

        if (compact_point_size < 4) {
            LOG(ERROR) << "compact_points not enough: " << compact_points.size();
            return;
        }

        PointT compact_point;
        compact_point = compact_points[2];
        int corner_size = int(compact_point.x);
        int surf_size = int(compact_point.y);
        int full_size = int(compact_point.z);

        if ((3 + corner_size + surf_size + full_size) != compact_point_size) {
            LOG(ERROR) << "compact data error: 3+" << corner_size << "+" << surf_size << "+" << full_size << " != "
                    << compact_point_size;
            return;
        }

        {
            compact_point = compact_points[0];
            transform_sum_.pos.x() = compact_point.x;
            transform_sum_.pos.y() = compact_point.y;
            transform_sum_.pos.z() = compact_point.z;
            compact_point = compact_points[1];
            transform_sum_.rot.x() = compact_point.x;
            transform_sum_.rot.y() = compact_point.y;
            transform_sum_.rot.z() = compact_point.z;
            transform_sum_.rot.w() = compact_point.intensity;
        }

        {
            laserCloudCornerLast->clear();
            laserCloudSurfLast->clear();
            laserCloudOutlierLastIntensity->clear();

            // TODO: use vector map instead?
            for (size_t i = 3; i < compact_point_size; ++i) {
                const PointT &p = compact_points[i];
                if (i < 3 + corner_size) {
                    laserCloudCornerLast->push_back(p);
                } else if (i >= 3 + corner_size && i < 3 + corner_size + surf_size) {
                    laserCloudSurfLast->push_back(p);
                } else {
                    laserCloudOutlierLastIntensity->push_back(p);
                }
            }
        }

        DLOG(INFO) << "decode compact data time: " << tic_toc_decoder.Toc() << " ms";
    }

    bool GetMapMeasurements() {
        if(compact_data_buf_.empty()) {
            return false;
        } else {
            while(!compact_data_buf_.empty()) {
                sensor_msgs::PointCloud2ConstPtr compact_data_msg = compact_data_buf_.front();
                compact_data_buf_.pop();
                compact_timestamps_.push(compact_data_msg->header.stamp);
                CompactDataHandler(compact_data_msg);
                compact_corner_clouds_.push(boost::make_shared<PointCloud>(*laserCloudCornerLast));
                compact_surf_clouds_.push(boost::make_shared<PointCloud>(*laserCloudSurfLast));
                compact_full_clouds_.push(boost::make_shared<PointCloud>(*laserCloudOutlierLastIntensity));
                compact_transform_sums_.push(transform_sum_);
                flag_rotation = true;
            }
            return true;
        }
    }

    void laserCloudOutlierLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        timeLaserCloudOutlierLast = msg->header.stamp.toSec();
//ROS_INFO("timeLaserCloudOutlierLast: %f", timeLaserCloudOutlierLast);
        laserCloudOutlierLast->clear();
        pcl::fromROSMsg(*msg, *laserCloudOutlierLast);
        newLaserCloudOutlierLast = true;
		
        laserCloudOutlierLastIntensity->clear();    //反射强度精简
        pcl::fromROSMsg(*msg, *laserCloudOutlierLastIntensity);
        LOG(INFO) << "get full cloud !!!!!!!!!!";
    }

    void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        timeLaserCloudCornerLast = msg->header.stamp.toSec();
        laserCloudCornerLast->clear();
        pcl::fromROSMsg(*msg, *laserCloudCornerLast);
        newLaserCloudCornerLast = true;
        LOG(INFO) << "get coner cloud !!!!!!!!!!";
    }

    void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        timeLaserCloudSurfLast = msg->header.stamp.toSec();
        laserCloudSurfLast->clear();
        pcl::fromROSMsg(*msg, *laserCloudSurfLast);
        newLaserCloudSurfLast = true;
        LOG(INFO) << "get surf cloud !!!!!!!!!!";
    }

    void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry){
        LOG(INFO) << "get odom !!!!!!!!!!";
        timeLaserOdometry = laserOdometry->header.stamp.toSec();
        geometry_msgs::Quaternion geo_quat = laserOdometry->pose.pose.orientation;

        transform_sum_.rot.w() = geo_quat.w;
        transform_sum_.rot.x() = geo_quat.x;
        transform_sum_.rot.y() = geo_quat.y;
        transform_sum_.rot.z() = geo_quat.z;

        transform_sum_.pos.x() = float(laserOdometry->pose.pose.position.x);
        transform_sum_.pos.y() = float(laserOdometry->pose.pose.position.y);
        transform_sum_.pos.z() = float(laserOdometry->pose.pose.position.z);

    /*ofstream out_file("/home/robot/iter/Save1.txt", ios::app);
    if(!out_file.is_open())
    {
        cout<<"fail to open the file!"<<endl;
    }
    out_file<<transformSum[0]<<" "<<transformSum[1]<<" "<<transformSum[2]<<std::endl;
    //out_file<<std::fixed<<imu_time<<std::endl;  
    out_file.close();*/

    // 	if(transformSum[0] > -2 && transformSum[0] < -0.5) zone1 = true;  //edit new start filter
    // 	if(transformSum[0] > -0.5 && transformSum[0] < 0.5) zone2 = true;
    // 	if(transformSum[0] > 0.5 && transformSum[0] < 2) zone3 = true;

    // 	if(transformSum[1] > -3.5 && transformSum[1] < -1) zone4 = true;  //edit new start filter
    // 	if(transformSum[1] > -1 && transformSum[1] < 1) zone5 = true;
    // 	if(transformSum[1] > 1 && transformSum[1] < 3.5) zone6 = true;

    // 	if((zone1 && zone2 && zone3) || (zone4 && zone5 && zone6))
    // 	{
    // 		Num_rotation++;
    // 		zone1 = false;
    // 		zone2 = false;
    // 		zone3 = false;
    // 		zone4 = false;
    // 		zone5 = false;
    // 		zone6 = false;
    // ROS_INFO("Num_rotation: %d", Num_rotation);
    // 	}

    // 	if(Num_rotation == 11) 
    // 	{
    // 		flag_rotation = true;
    // 	}
        flag_rotation = true;
        
        transform_odom_ = transform_sum_;  
        newLaserOdometry = true;
    }

    void updatePath(const PointTypePose& pose_in)
    {
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
        pose_stamped.header.frame_id = "world";
        pose_stamped.pose.position.x = pose_in.x;
        pose_stamped.pose.position.y = pose_in.y;
        pose_stamped.pose.position.z = pose_in.z;
        pose_stamped.pose.orientation.x = pose_in.qx;
        pose_stamped.pose.orientation.y = pose_in.qy;
        pose_stamped.pose.orientation.z = pose_in.qz;
        pose_stamped.pose.orientation.w = pose_in.qw;

        globalPath.poses.push_back(pose_stamped);
    }

    void publishTF(){

    pbtest++;

	// if(convergence == 1/* && mapNum % 3 != 0*/){
    if(1) {

//ROS_INFO("pbtest: %d", pbtest);
//ROS_INFO("flag_rotation: %d", flag_rotation);

        geometry_msgs::Quaternion geo_quat;
        geo_quat.w = transform_aft_mapped_.rot.w();
        geo_quat.x = transform_aft_mapped_.rot.x();
        geo_quat.y = transform_aft_mapped_.rot.y();
        geo_quat.z = transform_aft_mapped_.rot.z();

        odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);;
        odomAftMapped.pose.pose.orientation.x = geo_quat.x;
        odomAftMapped.pose.pose.orientation.y = geo_quat.y;
        odomAftMapped.pose.pose.orientation.z = geo_quat.z;
        odomAftMapped.pose.pose.orientation.w = geo_quat.w;
        odomAftMapped.pose.pose.position.x = transform_aft_mapped_.pos.x();
        odomAftMapped.pose.pose.position.y = transform_aft_mapped_.pos.y();
        odomAftMapped.pose.pose.position.z = transform_aft_mapped_.pos.z();
        pubOdomAftMapped.publish(odomAftMapped);

        aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry);
        aftMappedTrans.setRotation(tf::Quaternion(geo_quat.x, geo_quat.y, geo_quat.z, geo_quat.w));
        aftMappedTrans.setOrigin(tf::Vector3(transform_aft_mapped_.pos.x(),
                                            transform_aft_mapped_.pos.y(),
                                            transform_aft_mapped_.pos.z()));
        tfBroadcaster.sendTransform(aftMappedTrans);
		if (true){  //edit filter start rotation PC
        int lasersize = laserCloudOutlierLastIntensity->points.size();
		for (int i = 0; i < lasersize; i++) //edit  //反射强度精简
		{
			globalMap->push_back(laserCloudOutlierLastIntensity->points[i]);   //反射强度精简
			pointAssociateToMap(globalMap->points[i], globalMap->points[i], transform_aft_mapped_);
		}
		
		//ROS_INFO("OutlierLast.size: %d",laserCloudOutlierLast->points.size());
		//ROS_INFO("globalMap.size: %d",globalMap->points.size());		

        sensor_msgs::PointCloud2 cloudMsgTemp1;
		pcl::toROSMsg(*globalMap, cloudMsgTemp1);
        cloudMsgTemp1.header.stamp = ros::Time().fromSec(timeLaserOdometry);
        cloudMsgTemp1.header.frame_id = "world";
        pubLaserCloudmap.publish(cloudMsgTemp1); 
		}
		globalMap->clear();

        // publish path
        if (pubPath.getNumSubscribers() != 0)
        {
            globalPath.header.stamp = ros::Time().fromSec(timeLaserOdometry);
            globalPath.header.frame_id = "world";
            pubPath.publish(globalPath);
        }
	}
    }

    void publishKeyPosesAndFrames(){

        if (pubKeyPoses.getNumSubscribers() != 0){
            sensor_msgs::PointCloud2 cloudMsgTemp;
            pcl::toROSMsg(*cloudKeyPoses3D, cloudMsgTemp);
            cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
            cloudMsgTemp.header.frame_id = "world";
            pubKeyPoses.publish(cloudMsgTemp);
        }

        if (pubRecentKeyFrames.getNumSubscribers() != 0){
            sensor_msgs::PointCloud2 cloudMsgTemp;
            pcl::toROSMsg(*laserCloudSurfFromMapDS, cloudMsgTemp);
            cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
            cloudMsgTemp.header.frame_id = "world";
            pubRecentKeyFrames.publish(cloudMsgTemp);
        }
    }

    void visualizeGlobalMapThread(){
        ros::Rate rate(0.2);
        while (ros::ok()){
            rate.sleep();
            publishGlobalMap();
        }
    }

    void publishGlobalMap()
    {
        if (pubLaserCloudSurround.getNumSubscribers() == 0)
            return;

        if (cloudKeyPoses3D->points.empty() == true)
            return;

        pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;
        pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());

        // kd-tree to find near key frames to visualize
        std::vector<int> pointSearchIndGlobalMap;
        std::vector<float> pointSearchSqDisGlobalMap;
        // search near key frames to visualize
        mtx.lock();
        kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
        kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
        mtx.unlock();

        for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
            globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
        // downsample near selected key frames
        pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
        downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
        downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
        downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
        for(auto& pt : globalMapKeyPosesDS->points)
        {
            kdtreeGlobalMap->nearestKSearch(pt, 1, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);
            pt.intensity = cloudKeyPoses3D->points[pointSearchIndGlobalMap[0]].intensity;
        }

        // extract visualized and downsampled key frames
        for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i){
            if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
                continue;
            int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
            *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
            *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
        }
        // downsample visualized points
        pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
        downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
        publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, ros::Time().fromSec(timeLaserOdometry), "world");
    }

    // 接收外部的候选闭环信息，比如 scancontext 的候选信息
    void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg)
    {
        std::lock_guard<std::mutex> lock(mtxLoopInfo);
        if (loopMsg->data.size() != 2)
            return;

        loopInfoVec.push_back(*loopMsg);

        while (loopInfoVec.size() > 5)
            loopInfoVec.pop_front();
    }

    void loopClosureThread(){

        if (loopClosureEnableFlag == false)
            return;

        ros::Rate rate(loopClosureFrequency);
        while (ros::ok()){
            rate.sleep();
            performLoopClosure();
            visualizeLoopClosure();
        }
    }

    void performLoopClosure()
    {
        if (cloudKeyPoses3D->points.empty() == true)
            return;

        mtx.lock();
        *copy_cloudKeyPoses3D = *cloudKeyPoses3D;
        *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
        *copy_featurePoses = *featurePoses;
        mtx.unlock();

        // 检测逻辑：基于距离的方法（RS）搜索距离设置小一些，比如1m及以下，先按照RS找闭环候选，如果找到则进行后续的icp处理；如果RS没找到候选，则
        // 按照 iris 的方法找候选。RS 的条件设置的严格，用于处理里程计比较准的时候，当里程计不准的时候，用iris
        // find keys
        int loopKeyCur;
        int loopKeyPre;

        bool loopFlag = false;
        if (useIris) {
            loopFlag = detectLoopClosureDistanceAndIris(&loopKeyCur, &loopKeyPre);
        } else {
            loopFlag = detectLoopClosureDistance(&loopKeyCur, &loopKeyPre);
        }

        if (loopFlag == true) {
            // extract cloud
            pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
            pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
            {
                loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
                loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
                if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
                    return;
                if (pubHistoryKeyFrames.getNumSubscribers() != 0)
                    publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, ros::Time().fromSec(timeLaserOdometry), "world");
            }

            // ICP Settings
            static pcl::IterativeClosestPoint<PointType, PointType> icp;
            icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
            icp.setMaximumIterations(100);
            icp.setTransformationEpsilon(1e-6);
            icp.setEuclideanFitnessEpsilon(1e-6);
            icp.setRANSACIterations(0);

            // Align clouds
            icp.setInputSource(cureKeyframeCloud);
            icp.setInputTarget(prevKeyframeCloud);
            pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
            icp.align(*unused_result);

            if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
                return;

            // publish corrected cloud
            if (pubIcpKeyFrames.getNumSubscribers() != 0)
            {
                pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
                pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
                publishCloud(&pubIcpKeyFrames, closed_cloud, ros::Time().fromSec(timeLaserOdometry), "world");
            }

            // Get pose transformation
            float x, y, z, roll, pitch, yaw;
            Eigen::Affine3f correctionLidarFrame;
            correctionLidarFrame = icp.getFinalTransformation();
            // transform from world origin to wrong pose
            Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(copy_cloudKeyPoses6D->points[loopKeyCur]);
            // transform from world origin to corrected pose
            Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
            pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
            gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
            gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
            gtsam::Vector Vector6(6);
            float noiseScore = icp.getFitnessScore();
            Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
            noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

            // Add pose constraint
            mtx.lock();
            loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
            loopPoseQueue.push_back(poseFrom.between(poseTo));
            loopNoiseQueue.push_back(constraintNoise);
            mtx.unlock();

            // add loop constriant
            loopIndexContainer[loopKeyCur] = loopKeyPre;
        } else {
            // 判断是否找到 iris 闭环
            // loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
            // loopKeyPre = -1;

            // TicToc tic_tic;
            // irismanager->detectLoopClosureID(loopKeyCur, loopKeyPre);
            // LOG(INFO) << "detect loop closure time: " << tic_tic.Toc() << " ms";

            // if (loopKeyPre == -1) {
            //     return;
            // }
            // LOG(INFO) << "find loop, key cur: " << loopKeyCur << "   pre: " << loopKeyPre;

            // // extract cloud
            // pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
            // pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
            // {
            //     loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
            //     loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
            //     if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
            //         return;
            //     if (pubHistoryKeyFrames.getNumSubscribers() != 0)
            //         publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, ros::Time().fromSec(timeLaserOdometry), "world");
            // }

            // // ICP Settings
            // static pcl::IterativeClosestPoint<PointType, PointType> icp;
            // icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
            // icp.setMaximumIterations(100);
            // icp.setTransformationEpsilon(1e-6);
            // icp.setEuclideanFitnessEpsilon(1e-6);
            // icp.setRANSACIterations(0);

            // // Align clouds
            // icp.setInputSource(cureKeyframeCloud);
            // icp.setInputTarget(prevKeyframeCloud);
            // pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
            // icp.align(*unused_result);

            // if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
            //     return;

            // // publish corrected cloud
            // if (pubIcpKeyFrames.getNumSubscribers() != 0)
            // {
            //     pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
            //     pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
            //     publishCloud(&pubIcpKeyFrames, closed_cloud, ros::Time().fromSec(timeLaserOdometry), "world");
            // }

            // // Get pose transformation
            // float x, y, z, roll, pitch, yaw;
            // Eigen::Affine3f correctionLidarFrame;
            // correctionLidarFrame = icp.getFinalTransformation();
            // // transform from world origin to wrong pose
            // Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(copy_cloudKeyPoses6D->points[loopKeyCur]);
            // // transform from world origin to corrected pose
            // Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
            // pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
            // gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
            // gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
            // gtsam::Vector Vector6(6);
            // float noiseScore = icp.getFitnessScore();
            // Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
            // noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

            // // Add pose constraint
            // mtx.lock();
            // loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
            // loopPoseQueue.push_back(poseFrom.between(poseTo));
            // loopNoiseQueue.push_back(constraintNoise);
            // mtx.unlock();

            // // add loop constriant
            // loopIndexContainer[loopKeyCur] = loopKeyPre;
            
        }
    }

    bool detectLoopClosureDistance(int *latestID, int *closestID)
    {
        int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
        int loopKeyPre = -1;

        // check loop constraint added before
        auto it = loopIndexContainer.find(loopKeyCur);
        if (it != loopIndexContainer.end())
            return false;

        // find the closest history key frame
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
        kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
        kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
        
        for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
        {
            int id = pointSearchIndLoop[i];
            if (abs(copy_cloudKeyPoses6D->points[id].time - copy_cloudKeyPoses6D->back().time) > historyKeyframeSearchTimeDiff)
            {
                loopKeyPre = id;
                break;
            }
        }

        if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
            return false;

        *latestID = loopKeyCur;
        *closestID = loopKeyPre;

        return true;
    }

    bool detectLoopClosureDistanceAndIris(int *latestID, int *closestID)
    {
        if (copy_featurePoses->points.empty()) 
            return false;
 
        int loopKeyCur = lidarIris->frameIndexList[copy_featurePoses->points.size() - 1];
        int loopKeyPre = -1;

        // check loop constraint added before
        auto it = loopIndexContainer.find(loopKeyCur);
        if (it != loopIndexContainer.end())
            return false;

        // find the closest history key frame
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
        kdtreeHistoryKeyPoses->setInputCloud(copy_featurePoses);
        kdtreeHistoryKeyPoses->radiusSearch(copy_featurePoses->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
        // lidar iris
        {   
            float mindis = 1000;
            int loop_bias;
            for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
            {
                int id = pointSearchIndLoop[i];
                if (loopKeyCur - lidarIris->frameIndexList[id] < historyIrisFrameDiff) {
                    continue;
                }

                LidarIris::FeatureDesc &fd2 = lidarIris->featureList[id];
                int bias;
                auto dis = lidarIris->Compare(lidarIris->featureList[copy_featurePoses->points.size() - 1], fd2, &bias);
                if(dis < mindis) {
                    mindis = dis;
                    loopKeyPre = lidarIris->frameIndexList[id];
                    loop_bias = bias;
                    // loopID = j;
                }
            }

            if (loopKeyPre == -1 || mindis >= IrisHmThreshold)
                return false;
        }

        if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
            return false;

        *latestID = loopKeyCur;
        *closestID = loopKeyPre;

        return true;
    }

    bool detectLoopClosureExternal(int *latestID, int *closestID)
    {
        // this function is not used yet, please ignore it
        int loopKeyCur = -1;
        int loopKeyPre = -1;

        std::lock_guard<std::mutex> lock(mtxLoopInfo);
        if (loopInfoVec.empty())
            return false;

        double loopTimeCur = loopInfoVec.front().data[0];
        double loopTimePre = loopInfoVec.front().data[1];
        loopInfoVec.pop_front();

        if (abs(loopTimeCur - loopTimePre) < historyKeyframeSearchTimeDiff)
            return false;

        int cloudSize = copy_cloudKeyPoses6D->size();
        if (cloudSize < 2)
            return false;

        // latest key
        loopKeyCur = cloudSize - 1;
        for (int i = cloudSize - 1; i >= 0; --i)
        {
            if (copy_cloudKeyPoses6D->points[i].time >= loopTimeCur)
                loopKeyCur = round(copy_cloudKeyPoses6D->points[i].intensity);
            else
                break;
        }

        // previous key
        loopKeyPre = 0;
        for (int i = 0; i < cloudSize; ++i)
        {
            if (copy_cloudKeyPoses6D->points[i].time <= loopTimePre)
                loopKeyPre = round(copy_cloudKeyPoses6D->points[i].intensity);
            else
                break;
        }

        if (loopKeyCur == loopKeyPre)
            return false;

        auto it = loopIndexContainer.find(loopKeyCur);
        if (it != loopIndexContainer.end())
            return false;

        *latestID = loopKeyCur;
        *closestID = loopKeyPre;

        return true;
    }

    void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
    {
        // extract near keyframes
        nearKeyframes->clear();
        int cloudSize = copy_cloudKeyPoses6D->size();
        for (int i = -searchNum; i <= searchNum; ++i)
        {
            int keyNear = key + i;
            if (keyNear < 0 || keyNear >= cloudSize )
                continue;
            *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
            *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear],   &copy_cloudKeyPoses6D->points[keyNear]);
        }

        if (nearKeyframes->empty())
            return;

        // downsample near keyframes
        pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
        downSizeFilterICP.setInputCloud(nearKeyframes);
        downSizeFilterICP.filter(*cloud_temp);
        *nearKeyframes = *cloud_temp;
    }


    void visualizeLoopClosure()
    {
        if (loopIndexContainer.empty())
            return;
        
        visualization_msgs::MarkerArray markerArray;
        // loop nodes
        visualization_msgs::Marker markerNode;
        markerNode.header.frame_id = "world";
        markerNode.header.stamp = ros::Time().fromSec(timeLaserOdometry);
        markerNode.action = visualization_msgs::Marker::ADD;
        markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
        markerNode.ns = "loop_nodes";
        markerNode.id = 0;
        markerNode.pose.orientation.w = 1;
        markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3; 
        markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1;
        markerNode.color.a = 1;
        // loop edges
        visualization_msgs::Marker markerEdge;
        markerEdge.header.frame_id = "world";
        markerEdge.header.stamp = ros::Time().fromSec(timeLaserOdometry);
        markerEdge.action = visualization_msgs::Marker::ADD;
        markerEdge.type = visualization_msgs::Marker::LINE_LIST;
        markerEdge.ns = "loop_edges";
        markerEdge.id = 1;
        markerEdge.pose.orientation.w = 1;
        markerEdge.scale.x = 0.1;
        markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;
        markerEdge.color.a = 1;

        for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
        {
            int key_cur = it->first;
            int key_pre = it->second;
            geometry_msgs::Point p;
            p.x = copy_cloudKeyPoses6D->points[key_cur].x;
            p.y = copy_cloudKeyPoses6D->points[key_cur].y;
            p.z = copy_cloudKeyPoses6D->points[key_cur].z;
            markerNode.points.push_back(p);
            markerEdge.points.push_back(p);
            p.x = copy_cloudKeyPoses6D->points[key_pre].x;
            p.y = copy_cloudKeyPoses6D->points[key_pre].y;
            p.z = copy_cloudKeyPoses6D->points[key_pre].z;
            markerNode.points.push_back(p);
            markerEdge.points.push_back(p);
        }

        markerArray.markers.push_back(markerNode);
        markerArray.markers.push_back(markerEdge);
        pubLoopConstraintEdge.publish(markerArray);
    }




    Pose3 pclPointTogtsamPose3(PointTypePose thisPoint){
    	Eigen::Quaterniond q = Eigen::Quaternionf(thisPoint.qw, thisPoint.qx, thisPoint.qy, thisPoint.qz).template cast<double>();
        return Pose3(Rot3(q.normalized()),Point3(thisPoint.x, thisPoint.y, thisPoint.z));
    }

    Eigen::Affine3f pclPointToAffine3fCameraToLidar(PointTypePose thisPoint){
    	Eigen::Quaternionf q = Eigen::Quaternionf(thisPoint.qw, thisPoint.qx, thisPoint.qy, thisPoint.qz);
        Eigen::Vector3f euler = q.normalized().toRotationMatrix().eulerAngles(2, 1, 0);//得到World to Lidar欧拉角(z, y, x)
        Eigen::Vector3f thiseuler(euler.z(), euler.y(), euler.x());
        return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thiseuler.x(), thiseuler.y(), thiseuler.z());
    }

    void extractNearby()
    {   
        pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
        std::vector<int> pointSearchInd;
        std::vector<float> pointSearchSqDis;

        // extract all the nearby key poses and downsample them
        kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
        kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
        for (int i = 0; i < (int)pointSearchInd.size(); ++i)
        {
            int id = pointSearchInd[i];
            surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[id]);
        }
        // *surroundingKeyPosesDS = *surroundingKeyPoses;
        downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
        for(auto& pt : surroundingKeyPosesDS->points)
        {
            kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
            pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
        }
        // also extract some latest key frames in case the robot rotates in one position
        int numPoses = cloudKeyPoses3D->size();
        for (int i = numPoses-1; i >= 0; --i)
        {
            if (timeLaserOdometry - cloudKeyPoses6D->points[i].time < 10.0) {
                surroundingKeyPosesDS->points.push_back(cloudKeyPoses3D->points[i]);
            }
            else
                break;
        }

        extractCloud(surroundingKeyPosesDS);
    }

    void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
    {
        // fuse the map
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear(); 
        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {
            if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
                continue;

            int thisKeyInd = (int)cloudToExtract->points[i].intensity;
            if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end()) 
            {
                // transformed cloud available
                *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
                *laserCloudSurfFromMap   += laserCloudMapContainer[thisKeyInd].second;
            } else {
                // transformed cloud not available
                pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
                pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
                *laserCloudCornerFromMap += laserCloudCornerTemp;
                *laserCloudSurfFromMap   += laserCloudSurfTemp;
                laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
            }
            
        }

        // Downsample the surrounding corner key frames (or map)
        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
        // Downsample the surrounding surf key frames (or map)
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
        laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();

        // clear map cache if too large
        if (laserCloudMapContainer.size() > 1000)
            laserCloudMapContainer.clear();
    }

    void extractSurroundingKeyFrames(){

        if (cloudKeyPoses3D->points.empty() == true)
            return;	
		
        extractNearby();
    }

    void downsampleCurrentScan(){

        laserCloudCornerLastDS->clear();
        downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
        downSizeFilterCorner.filter(*laserCloudCornerLastDS);
        laserCloudCornerLastDSNum = laserCloudCornerLastDS->points.size();

        //laserCloudCornerLastDSformap->clear();  //edit downsize 2nd
        //downSizeFilterCornerformap.setInputCloud(laserCloudCornerLast);
        //downSizeFilterCornerformap.filter(*laserCloudCornerLastDSformap);

//ROS_INFO("==========cornernum: %d", laserCloudCornerLast->points.size());
//ROS_INFO("==========cornernumDS: %d", laserCloudCornerLastDS->points.size());

        laserCloudSurfLastDS->clear();
        downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
        downSizeFilterSurf.filter(*laserCloudSurfLastDS);
        laserCloudSurfLastDSNum = laserCloudSurfLastDS->points.size();

        //laserCloudSurfLastDSformap->clear();  //edit downsize 2nd
        //downSizeFilterSurfformap.setInputCloud(laserCloudSurfLast);
        //downSizeFilterSurfformap.filter(*laserCloudSurfLastDSformap);

//ROS_INFO("==========surfnum: %d", laserCloudSurfLast->points.size());
//ROS_INFO("==========surfnumDS: %d", laserCloudSurfLastDS->points.size());

        laserCloudOutlierLastDS->clear();
        downSizeFilterOutlier.setInputCloud(laserCloudOutlierLast);
        downSizeFilterOutlier.filter(*laserCloudOutlierLastDS);
        laserCloudOutlierLastDSNum = laserCloudOutlierLastDS->points.size();

        //laserCloudOutlierLastDSformap->clear();  //edit downsize 2nd
        //downSizeFilterOutlierformap.setInputCloud(laserCloudOutlierLast);
        //downSizeFilterOutlierformap.filter(*laserCloudOutlierLastDSformap);

        /*laserCloudSurfTotalLast->clear();
        laserCloudSurfTotalLastDS->clear();
        *laserCloudSurfTotalLast += *laserCloudSurfLastDS;
        *laserCloudSurfTotalLast += *laserCloudOutlierLastDS;
        downSizeFilterSurf.setInputCloud(laserCloudSurfTotalLast);*/  //edit feature point

	//pcl::copyPointCloud(*laserCloudSurfLastDS, *laserCloudSurfTotalLastDS);  //edit feature point

        //downSizeFilterSurf.filter(*laserCloudSurfTotalLastDS);
        //laserCloudSurfTotalLastDSNum = laserCloudSurfTotalLastDS->points.size();  //edit downsize DS  //edit feature point
    }

    void cornerOptimization(int iterCount, ceres::Problem &problem, ceres::LossFunction *loss_function){

        for (int i = 0; i < laserCloudCornerLastDSNum; i++) {
            pointOri = laserCloudCornerLastDS->points[i];
            pointAssociateToMap(pointOri, pointSel, transform_tobe_mapped_);
            kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
            
            if (pointSearchSqDis[4] < min_match_sq_dis) {
                float cx = 0, cy = 0, cz = 0;
                for (int j = 0; j < 5; j++) {
                    cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                    cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                    cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                }
                cx /= 5; cy /= 5;  cz /= 5;

                float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                for (int j = 0; j < 5; j++) {
                    float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                    float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                    float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;

                    a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
                    a22 += ay * ay; a23 += ay * az;
                    a33 += az * az;
                }
                a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;

                matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
                matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
                matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;

                cv::eigen(matA1, matD1, matV1);

                if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {

                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;
                    float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                    float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                    float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                    float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                    float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                    float z2 = cz - 0.1 * matV1.at<float>(0, 2);

                    Eigen::Vector3d X0(pointOri.x, pointOri.y, pointOri.z);
                    Eigen::Vector3d X1(x1, y1, z1);
                    Eigen::Vector3d X2(x2, y2, z2);

                    ceres::CostFunction *cost_function = LioLidarEdgeFactorPose::Create(X0, X1, X2, 1.0); 
                    problem.AddResidualBlock(cost_function, loss_function, lioposeparam);
                }
            }
        }
    }

    void surfOptimization(int iterCount, ceres::Problem &problem, ceres::LossFunction *loss_function){
        for (int i = 0; i < laserCloudSurfLastDSNum; i++) {  //edit downsize laserCloudSurfTotalLastDSNum  //edit feature point
            pointOri = laserCloudSurfLastDS->points[i];  //edit downsize laserCloudSurfTotalLastDS  //edit feature point
            pointAssociateToMap(pointOri, pointSel, transform_tobe_mapped_); 
            kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            if (pointSearchSqDis[4] < min_match_sq_dis) {
                for (int j = 0; j < 5; j++) {
                    matA0.at<float>(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                    matA0.at<float>(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                    matA0.at<float>(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                }
                cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);

                float pa = matX0.at<float>(0, 0);
                float pb = matX0.at<float>(1, 0);
                float pc = matX0.at<float>(2, 0);
                float pd = 1;

                float ps = sqrt(pa * pa + pb * pb + pc * pc);
                pa /= ps; pb /= ps; pc /= ps; pd /= ps;

                bool planeValid = true;
                for (int j = 0; j < 5; j++) {
                    if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                             pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                             pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > min_plane_dis) {
                        planeValid = false;
                        break;
                    }
                }

                if (planeValid) {
                    Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); 
                    Eigen::Vector3d norm(pa, pb, pc); 
                    ceres::CostFunction *cost_function = LioLidarPlaneNormFactor::Create(curr_point, norm, double(pd)); 
                    problem.AddResidualBlock(cost_function, loss_function, lioposeparam);
                }
            }
        }
    }

    bool LMOptimization(int iterCount){
        int laser_cloud_sel_size = laserCloudOri->points.size();
        if (laser_cloud_sel_size < 50) {
            return false;
        }

        Eigen::Matrix<float, Eigen::Dynamic, 6> mat_A(laser_cloud_sel_size, 6);
        Eigen::Matrix<float, 6, Eigen::Dynamic> mat_At(6, laser_cloud_sel_size);
        Eigen::Matrix<float, 6, 6> matAtA;
        Eigen::VectorXf mat_B(laser_cloud_sel_size);
        Eigen::VectorXf mat_AtB;
        Eigen::VectorXf mat_X;

        SO3 R_SO3(transform_tobe_mapped_.rot); /// SO3
        PointType point_ori;
        for (int i = 0; i < laser_cloud_sel_size; i++) {
        point_ori = laserCloudOri->points[i];
        coeff = coeffSel->points[i];

        Eigen::Vector3f p(point_ori.x, point_ori.y, point_ori.z);
        Eigen::Vector3f w(coeff.x, coeff.y, coeff.z);
        Eigen::Vector3f J_r = -w.transpose() * (transform_tobe_mapped_.rot * SkewSymmetric(p))
            * transform_tobe_mapped_.rot.inverse().toRotationMatrix();
        Eigen::Vector3f J_t = w.transpose();

        float d2 = coeff.intensity;

        mat_A(i, 0) = J_r.x();
        mat_A(i, 1) = J_r.y();
        mat_A(i, 2) = J_r.z();
        mat_A(i, 3) = J_t.x();
        mat_A(i, 4) = J_t.y();
        mat_A(i, 5) = J_t.z();
        mat_B(i, 0) = -d2;
        }

        mat_At = mat_A.transpose();
        matAtA = mat_At * mat_A;
        mat_AtB = mat_At * mat_B;
        mat_X = matAtA.colPivHouseholderQr().solve(mat_AtB);

        if (iterCount == 0) {
        Eigen::Matrix<float, 1, 6> mat_E;
        Eigen::Matrix<float, 6, 6> mat_V;
        Eigen::Matrix<float, 6, 6> mat_V2;

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 6, 6> > esolver(matAtA);
        mat_E = esolver.eigenvalues().real();
        mat_V = esolver.eigenvectors().real();

        mat_V2 = mat_V;

        isDegenerate = false;
        float eignThre[6] = {100, 100, 100, 100, 100, 100};
        for (int i = 0; i < 6; ++i) {
            if (mat_E(0, i) < eignThre[i]) {
            for (int j = 0; j < 6; ++j) {
                mat_V2(i, j) = 0;
            }
            isDegenerate = true;
            DLOG(WARNING) << "degenerate case";
            DLOG(INFO) << mat_E;
            } else {
            break;
            }
        }
        matP_ = mat_V2 * mat_V.inverse();
        }

        if (isDegenerate) {
        Eigen::Matrix<float, 6, 1> matX2(mat_X);
        mat_X = matP_ * matX2;
        }

        Eigen::Vector3f r_so3 = R_SO3.log();

        r_so3.x() += mat_X(0, 0);
        r_so3.y() += mat_X(1, 0);
        r_so3.z() += mat_X(2, 0);
        transform_tobe_mapped_.pos.x() += mat_X(3, 0);
        transform_tobe_mapped_.pos.y() += mat_X(4, 0);
        transform_tobe_mapped_.pos.z() += mat_X(5, 0);

        if (!isfinite(r_so3.x())) r_so3.x() = 0;
        if (!isfinite(r_so3.y())) r_so3.y() = 0;
        if (!isfinite(r_so3.z())) r_so3.z() = 0;

        SO3 tobe_mapped_SO3 = SO3::exp(r_so3);
        transform_tobe_mapped_.rot =
            (DeltaQ(Eigen::Vector3f(mat_X(0, 0), mat_X(1, 0), mat_X(2, 0))) * transform_tobe_mapped_.rot);

        if (!isfinite(transform_tobe_mapped_.pos.x())) transform_tobe_mapped_.pos.x() = 0.0;
        if (!isfinite(transform_tobe_mapped_.pos.y())) transform_tobe_mapped_.pos.y() = 0.0;
        if (!isfinite(transform_tobe_mapped_.pos.z())) transform_tobe_mapped_.pos.z() = 0.0;

        float delta_r = RadToDeg(R_SO3.unit_quaternion().angularDistance(transform_tobe_mapped_.rot));
        float delta_t = sqrt(pow(mat_X(3, 0) * 100, 2) +
            pow(mat_X(4, 0) * 100, 2) +
            pow(mat_X(5, 0) * 100, 2));

        if (delta_r < 0.05 && delta_t < 0.05) {
            convergence = 1;
            DLOG(INFO) << "iter_count: " << iterCount;
            return true;
        }

		if(iterCount > 20){
            ROS_INFO("iterCount: %d", iterCount);
            ROS_INFO("deltaR: %f", delta_r);
            ROS_INFO("deltaT: %f", delta_t);
        }

        return false;
    }

    void scan2MapOptimization(){

    int mapsize = laserCloudCornerFromMapDSNum + laserCloudSurfFromMapDSNum;
    int framesize = laserCloudCornerLastDSNum + laserCloudSurfLastDSNum;

//ROS_INFO("==========framecornersize: %d", laserCloudCornerLastDSNum);
//ROS_INFO("==========framesurfsize: %d", laserCloudSurfLastDSNum);
//ROS_INFO("==========frametotalsize: %d", framesize);
//ROS_INFO("==========mapcornersize: %d", laserCloudCornerFromMapDSNum);
//ROS_INFO("==========mapsurfsize: %d", laserCloudSurfFromMapDSNum);
//ROS_INFO("==========maptotalsize: %d", mapsize);
        
        //  旋转约束与非旋转约束都使用四元数 

        if (laserCloudCornerFromMapDSNum > 5 && laserCloudSurfFromMapDSNum > 100) {  //edit filter 100 lasers

            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

	        convergence = 0;
            int maximumiter = 4;
            for (int iterCount = 0; iterCount < maximumiter; iterCount++) {
                
                ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0); 
                ceres::LocalParameterization *q_parameterization = new NoConstrainLocalParameterization(); 
                ceres::Problem::Options problem_options; 
                ceres::Problem problem(problem_options); 
                problem.AddParameterBlock(lioposeparam, 7, q_parameterization); 
                lioposeparam[0] = transform_tobe_mapped_.rot.x();
                lioposeparam[1] = transform_tobe_mapped_.rot.y();
                lioposeparam[2] = transform_tobe_mapped_.rot.z();
                lioposeparam[3] = transform_tobe_mapped_.rot.w();
                lioposeparam[4] = transform_tobe_mapped_.pos.x();
                lioposeparam[5] = transform_tobe_mapped_.pos.y();
                lioposeparam[6] = transform_tobe_mapped_.pos.z();

                laserCloudOri->clear();
                coeffSel->clear();

                cornerOptimization(iterCount, problem, loss_function);
                surfOptimization(iterCount, problem, loss_function);
                
                ceres::Solver::Options options; 
                options.linear_solver_type = ceres::DENSE_QR; 
                options.max_num_iterations = 4; 
                options.minimizer_progress_to_stdout = false; 
                options.check_gradients = false; 
                options.gradient_check_relative_precision = 1e-4; 
                ceres::Solver::Summary summary; 
                ceres::Solve(options, &problem, &summary); 
                //std::cout << "lego_loam===" << summary.BriefReport() << std::endl;
                transform_tobe_mapped_.rot.x() = lioposeparam[0];
                transform_tobe_mapped_.rot.y() = lioposeparam[1];
                transform_tobe_mapped_.rot.z() = lioposeparam[2];
                transform_tobe_mapped_.rot.w() = lioposeparam[3];
                transform_tobe_mapped_.pos.x() = lioposeparam[4];
                transform_tobe_mapped_.pos.y() = lioposeparam[5];
                transform_tobe_mapped_.pos.z() = lioposeparam[6];

                // if (LMOptimization(iterCount) == true)
                //     break;              
            }

		    transformUpdate();

        }else{
	    transformUpdate();  //edit filter 100 lasers
	}
    }

    void VectortoDouble() {
        // 优化位姿
        for(int i = 0; i < frame_size; ++i) {
            para_pose_[i][0] = all_laser_transforms_[i].rot.x();
            para_pose_[i][1] = all_laser_transforms_[i].rot.y();
            para_pose_[i][2] = all_laser_transforms_[i].rot.z();
            para_pose_[i][3] = all_laser_transforms_[i].rot.w();
            para_pose_[i][4] = all_laser_transforms_[i].pos.x();
            para_pose_[i][5] = all_laser_transforms_[i].pos.y();
            para_pose_[i][6] = all_laser_transforms_[i].pos.z();
        } 
    }

    void DoubletoVector() {
        // 优化位姿
        for(int i = 0; i < frame_size; ++i) {
            all_laser_transforms_[i].rot.x() = para_pose_[i][0];
            all_laser_transforms_[i].rot.y() = para_pose_[i][1];
            all_laser_transforms_[i].rot.z() = para_pose_[i][2];
            all_laser_transforms_[i].rot.w() = para_pose_[i][3];
            all_laser_transforms_[i].pos.x() = para_pose_[i][4];
            all_laser_transforms_[i].pos.y() = para_pose_[i][5];
            all_laser_transforms_[i].pos.z() = para_pose_[i][6];
        }
    }
    
    void Multiscan2MapOptimization() { 
        TicToc tic_toc_;
        tic_toc_.Tic();       
        //  旋转约束与非旋转约束都使用四元数 
        if (laserCloudCornerFromMapDSNum > 5 && laserCloudSurfFromMapDSNum > 100) {  //edit filter 100 lasers

            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

            for (int i = 0; i < frame_size; ++i) {
                LOG(INFO) << "lego before opt trans[" << i << "] = " << all_laser_transforms_[i].pos.transpose();
            }

	        convergence = 0;
            int maximumiter = 5;
            for (int iterCount = 0; iterCount < maximumiter; iterCount++) {
                
                ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);  
                ceres::Problem::Options problem_options; 
                ceres::Problem problem(problem_options); 
                for (int i = 0; i < frame_size; ++i) {
                    if (use_analytic) {
                        ceres::LocalParameterization *local_parameterization = new AnalyticPoseLocalParameterization();
                        problem.AddParameterBlock(para_pose_[i], 7, local_parameterization);
                    } else {
                        ceres::LocalParameterization *local_parameterization = new NoConstrainLocalParameterization();
                        problem.AddParameterBlock(para_pose_[i], 7, local_parameterization);
                    }
                }

                laserCloudOri->clear();
                coeffSel->clear();

                VectortoDouble();

                for(int k = 0; k < frame_size; ++k) {

                    int coner_stack_size = corner_stack_[k]->size();
                    int surf_stack_size = surf_stack_[k]->size();
                    transform_tobe_mapped_ = all_laser_transforms_[k];

                    LOG(INFO) << "corner_stack_[" << k << "].size = " << coner_stack_size;
                    LOG(INFO) << "surf_stack_[" << k << "].size = " << surf_stack_size;
                    // cornerOptimization
                    for (int i = 0; i < coner_stack_size; i++) {
                        pointOri = corner_stack_[k]->points[i];
                        pointAssociateToMap(pointOri, pointSel, transform_tobe_mapped_);
                        kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
                        
                        if (pointSearchSqDis[4] < min_match_sq_dis) {
                            float cx = 0, cy = 0, cz = 0;
                            for (int j = 0; j < 5; j++) {
                                cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                                cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                                cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                            }
                            cx /= 5; cy /= 5;  cz /= 5;

                            float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                            for (int j = 0; j < 5; j++) {
                                float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                                float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                                float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;

                                a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
                                a22 += ay * ay; a23 += ay * az;
                                a33 += az * az;
                            }
                            a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;

                            matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
                            matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
                            matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;

                            cv::eigen(matA1, matD1, matV1);

                            if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {

                                float x0 = pointSel.x;
                                float y0 = pointSel.y;
                                float z0 = pointSel.z;
                                float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                                float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                                float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                                float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                                float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                                float z2 = cz - 0.1 * matV1.at<float>(0, 2);

                                Eigen::Vector3d X0(pointOri.x, pointOri.y, pointOri.z);
                                Eigen::Vector3d X1(x1, y1, z1);
                                Eigen::Vector3d X2(x2, y2, z2);

                                if (use_analytic) {
                                    AnalyticLidarEdgeFactor *cost_function = new AnalyticLidarEdgeFactor(X0, X1, X2); 
                                    problem.AddResidualBlock(cost_function, loss_function, para_pose_[k]);
                                } else {
                                    ceres::CostFunction *cost_function = LioLidarEdgeFactorPose::Create(X0, X1, X2, 1.0); 
                                    problem.AddResidualBlock(cost_function, loss_function, para_pose_[k]);
                                }
                            }
                        }
                    }
                    // surfOptimization
                    for (int i = 0; i < surf_stack_size; i++) {  //edit downsize laserCloudSurfTotalLastDSNum  //edit feature point
                        pointOri = surf_stack_[k]->points[i];  //edit downsize laserCloudSurfTotalLastDS  //edit feature point
                        pointAssociateToMap(pointOri, pointSel, transform_tobe_mapped_); 
                        kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

                        if (pointSearchSqDis[4] < min_match_sq_dis) {
                            for (int j = 0; j < 5; j++) {
                                matA0.at<float>(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                                matA0.at<float>(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                                matA0.at<float>(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                            }
                            cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);

                            float pa = matX0.at<float>(0, 0);
                            float pb = matX0.at<float>(1, 0);
                            float pc = matX0.at<float>(2, 0);
                            float pd = 1;

                            float ps = sqrt(pa * pa + pb * pb + pc * pc);
                            pa /= ps; pb /= ps; pc /= ps; pd /= ps;

                            bool planeValid = true;
                            for (int j = 0; j < 5; j++) {
                                if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                                        pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                                        pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > min_plane_dis) {
                                    planeValid = false;
                                    break;
                                }
                            }

                            if (planeValid) {
                                Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); 
                                Eigen::Vector3d norm(pa, pb, pc); 
                                if (use_analytic) {
                                    AnalyticLidarPlaneNormFactor *cost_function = new AnalyticLidarPlaneNormFactor(curr_point, norm, double(pd)); 
                                    problem.AddResidualBlock(cost_function, loss_function, para_pose_[k]);
                                } else {
                                    ceres::CostFunction *cost_function = LioLidarPlaneNormFactor::Create(curr_point, norm, double(pd)); 
                                    problem.AddResidualBlock(cost_function, loss_function, para_pose_[k]);
                                }
                            }
                        }
                    }
                }
                ceres::Solver::Options options; 
                // options.linear_solver_type = ceres::DENSE_QR; 
                options.linear_solver_type = ceres::DENSE_SCHUR; 
                options.trust_region_strategy_type = ceres::DOGLEG;
                // options.max_num_iterations = 4; 
                // options.function_tolerance = 1e-12;
                // options.parameter_tolerance = 1e-12;
                // options.gradient_tolerance = 1e-12;
                options.minimizer_progress_to_stdout = true; 
                options.check_gradients = false; 
                options.gradient_check_relative_precision = 1e-4; 
                ceres::Solver::Summary summary;
                TicToc ceres_sovler_tictoc;
                ceres_sovler_tictoc.Tic(); 
                ceres::Solve(options, &problem, &summary); 
                std::cout << "multi lego_loam===" << summary.FullReport() << std::endl;
                DoubletoVector();
                static double opt_time_ave_cost = 0.0;
                static int opt_counts = 0;
                opt_time_ave_cost += ceres_sovler_tictoc.Toc();
                opt_counts++;
                LOG(INFO) << "average one ceres slove cost time: " << opt_time_ave_cost / opt_counts;
                LOG(INFO) <<"t_opt: " << tic_toc_.Toc() << " ms";      
            }
            for (int i = 0; i < frame_size; ++i) {
                DLOG(INFO) << "lego after opt trans[" << i << "] = " << all_laser_transforms_[i].pos.transpose();
            }		    
        }
    }
	
	void ProcessMap() {

	  if (odom_count_ % 1 == 0) {  //odom_count_ =0; skip_count_ = 2; 每两帧匹配一次
		if (1) {  //enable_4d_  //edit
		  OptimizeMap();
		} else {
		  //OptimizeTransformTobeMapped();
		  transformUpdate();
		  DLOG(INFO) << "DISABLE 4D";
		}
	  } else {
		if (1) {  //enable_4d_  //edit
		  //Transform4DUpdate();  //结束后的更新旋转平移参数
		  transformUpdate();
		} else {
		  transformUpdate();
		  DLOG(INFO) << "DISABLE 4D";
		}
	  }
	  ++odom_count_;
	}

	void OptimizeMap() {

	  convergence = 0;
//ROS_INFO("corner: %d", laserCloudCornerFromMapDS->points.size());
//ROS_INFO("surf: %d", laserCloudSurfFromMapDS->points.size());
		
	  //if (laser_cloud_corner_from_map_->points.size() <= 10 || laser_cloud_surf_from_map_->points.size() <= 100) {
	  if (laserCloudCornerFromMapDS->points.size() <= 10 || laserCloudSurfFromMapDS->points.size() <= 100) {  //已经是全局坐标了
		LOG(ERROR) << "skip due to insufficient points";
		transformUpdate();  //edit filter 100 lasers
		return;
	  }
      LOG(INFO) << "OptimizeMap()";
	  PointT point_sel, point_ori, coeff_, abs_coeff;

	  std::vector<int> point_search_idx(5, 0);
	  std::vector<float> point_search_sq_dis(5, 0);

	  //pcl::KdTreeFLANN<PointT>::Ptr kdtree_corner_from_map(new pcl::KdTreeFLANN<PointT>());
	  //pcl::KdTreeFLANN<PointT>::Ptr kdtree_surf_from_map(new pcl::KdTreeFLANN<PointT>());

	  //kdtree_corner_from_map->setInputCloud(laser_cloud_corner_from_map_);
	  //kdtree_surf_from_map->setInputCloud(laser_cloud_surf_from_map_);
	  kdtreeCornerFromMap1->setInputCloud(laserCloudCornerFromMapDS);
	  kdtreeSurfFromMap1->setInputCloud(laserCloudSurfFromMapDS);

	  Eigen::Matrix<float, 5, 3> mat_A0;
	  Eigen::Matrix<float, 5, 1> mat_B0;
	  Eigen::Vector3f mat_X0;
	  Eigen::Matrix3f mat_A1;
	  Eigen::Matrix<float, 1, 3> mat_D1;
	  Eigen::Matrix3f mat_V1;

	  mat_A0.setZero();
	  mat_B0.setConstant(-1);
	  mat_X0.setZero();

	  mat_A1.setZero();
	  mat_D1.setZero();
	  mat_V1.setZero();

	  bool is_degenerate = false;
	  matP_.setIdentity();

	  //size_t laser_cloud_corner_stack_size = laser_cloud_corner_stack_downsampled_->points.size();
	  //size_t laser_cloud_surf_stack_size = laser_cloud_surf_stack_downsampled_->points.size();
	  //size_t laser_cloud_corner_stack_size = laserCloudCornerLastDS->points.size();  //当前帧角点
	  //size_t laser_cloud_surf_stack_size = laserCloudSurfTotalLastDS->points.size();  //当前帧面点

	  PointCloud laser_cloud_ori;
	  PointCloud coeff_sel;

	  PointCloud laser_cloud_ori_spc;
	  score_point_coeff_.clear();

	  //tic_toc_.Tic();  //edit

	  Eigen::Quaternionf q_R = transform_sum_.rot.normalized();
	  Eigen::Vector3f constrained_axis = ConstrainedRotAxis(q_R);
	  constrained_axis.normalize();
	  Eigen::Matrix3f constrained_R =
		  Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f{1.0, 0.0, 0.0}, constrained_axis).toRotationMatrix();
	  Eigen::Matrix3f info_mat;
	  info_mat.setZero();
	  info_mat(0, 0) = 1;
	  info_mat(1, 1) = 1;
	  info_mat(2, 2) = 1;
	  info_mat = (constrained_R.transpose() * info_mat * constrained_R).eval();
	  Eigen::Matrix3f projection_mat =
		  constrained_axis * (constrained_axis.transpose() * constrained_axis).inverse() * constrained_axis.transpose();

		size_t maximumiter = 4;
	  for (size_t iter_count = 0; iter_count < maximumiter; ++iter_count) {  //num_max_iterations_ = 10；  //edit 
		laser_cloud_ori.clear();
		coeff_sel.clear();

		laser_cloud_ori_spc.clear();
        ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0); 
        ceres::LocalParameterization *q_parameterization = new AutoPoseLocalParameterization(); 
        // ceres::LocalParameterization *q_parameterization = new NoConstrainLocalParameterization(); 
        ceres::Problem::Options problem_options; 
        ceres::Problem problem(problem_options); 
        problem.AddParameterBlock(lioposeparam, 7, q_parameterization); 
        lioposeparam[0] = transform_tobe_mapped_.rot.x();
        lioposeparam[1] = transform_tobe_mapped_.rot.y();
        lioposeparam[2] = transform_tobe_mapped_.rot.z();
        lioposeparam[3] = transform_tobe_mapped_.rot.w();
        lioposeparam[4] = transform_tobe_mapped_.pos.x();
        lioposeparam[5] = transform_tobe_mapped_.pos.y();
        lioposeparam[6] = transform_tobe_mapped_.pos.z();

		//for (int i = 0; i < laser_cloud_corner_stack_size; ++i) {  //角点迭代
		for (int i = 0; i < laserCloudCornerLastDSNum; ++i) {  //角点迭代
		  point_ori = laserCloudCornerLastDS->points[i];
		  pointAssociateToMap(point_ori, point_sel, transform_tobe_mapped_);
		  //pointAssociateToMap(&pointOri, &pointSel);
		//   pointAssociateToMap(&point_ori, &point_sel);
		  //kdtree_corner_from_map->nearestKSearch(point_sel, 5, point_search_idx, point_search_sq_dis);
		  //kdtreeCornerFromMap1->nearestKSearch(point_sel, 5, point_search_idx, point_search_sq_dis);
		  kdtreeCornerFromMap1->nearestKSearch(point_sel, 5, pointSearchInd, pointSearchSqDis);

		  if (pointSearchSqDis[4] < min_match_sq_dis) {  //min_match_sq_dis_ = 1.0；  //edit
	//        Vector3Intl vc(0, 0, 0);
			Eigen::Vector3f vc(0, 0, 0);

			for (int j = 0; j < 5; j++) {
	//          vc += Vector3Intl(laser_cloud_corner_from_map_->points[point_search_idx[j]]);
			  //const PointT &point_sel_tmp = laser_cloud_corner_from_map_->points[point_search_idx[j]];
			  const PointT &point_sel_tmp = laserCloudCornerFromMapDS->points[pointSearchInd[j]];
			  vc.x() += point_sel_tmp.x;
			  vc.y() += point_sel_tmp.y;
			  vc.z() += point_sel_tmp.z;
			}
			vc /= 5.0;

			Eigen::Matrix3f mat_a;
			mat_a.setZero();

			for (int j = 0; j < 5; j++) {
	//          Vector3Intl a = Vector3Intl(laser_cloud_corner_from_map_->points[point_search_idx[j]]) - vc;
			  //const PointT &point_sel_tmp = laser_cloud_corner_from_map_->points[point_search_idx[j]];
			  const PointT &point_sel_tmp = laserCloudCornerFromMapDS->points[pointSearchInd[j]];
			  Eigen::Vector3f a;
			  a.x() = point_sel_tmp.x - vc.x();
			  a.y() = point_sel_tmp.y - vc.y();
			  a.z() = point_sel_tmp.z - vc.z();

			  mat_a(0, 0) += a.x() * a.x();
			  mat_a(1, 0) += a.x() * a.y();
			  mat_a(2, 0) += a.x() * a.z();
			  mat_a(1, 1) += a.y() * a.y();
			  mat_a(2, 1) += a.y() * a.z();
			  mat_a(2, 2) += a.z() * a.z();
			}
			mat_A1 = mat_a / 5.0;

			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(mat_A1);
			mat_D1 = esolver.eigenvalues().real();
			mat_V1 = esolver.eigenvectors().real();

			if (mat_D1(0, 2) > 3 * mat_D1(0, 1)) {

			  float x0 = point_sel.x;
			  float y0 = point_sel.y;
			  float z0 = point_sel.z;
			  float x1 = vc.x() + 0.1 * mat_V1(0, 2);
			  float y1 = vc.y() + 0.1 * mat_V1(1, 2);
			  float z1 = vc.z() + 0.1 * mat_V1(2, 2);
			  float x2 = vc.x() - 0.1 * mat_V1(0, 2);
			  float y2 = vc.y() - 0.1 * mat_V1(1, 2);
			  float z2 = vc.z() - 0.1 * mat_V1(2, 2);

			  Eigen::Vector3d X0(point_ori.x, point_ori.y, point_ori.z);
			  Eigen::Vector3d X1(x1, y1, z1);
			  Eigen::Vector3d X2(x2, y2, z2);

              ceres::CostFunction *cost_function = LioLidarEdgeFactorPose::Create(X0, X1, X2, 1.0); 
			  problem.AddResidualBlock(cost_function, loss_function, lioposeparam);
			}
		  }
		}

		//for (int i = 0; i < laser_cloud_surf_stack_size; i++) {  //面点迭代
		for (int i = 0; i < laserCloudSurfLastDSNum; i++) {  //面点迭代  //edit downsize laserCloudSurfTotalLastDSNum  //edit feature point
		  //point_ori = laser_cloud_surf_stack_downsampled_->points[i];
		  point_ori = laserCloudSurfLastDS->points[i];  //edit downsize laserCloudSurfTotalLastDS   //edit feature point
		  pointAssociateToMap(point_ori, point_sel, transform_tobe_mapped_);
		//   pointAssociateToMap(&point_ori, &point_sel);

		  int num_neighbors = 5;
		  //kdtree_surf_from_map->nearestKSearch(point_sel, num_neighbors, point_search_idx, point_search_sq_dis);
		  //kdtreeSurfFromMap1->nearestKSearch(point_sel, num_neighbors, point_search_idx, point_search_sq_dis);
		  kdtreeSurfFromMap1->nearestKSearch(point_sel, num_neighbors, pointSearchInd, pointSearchSqDis);

		  if (pointSearchSqDis[num_neighbors - 1] < min_match_sq_dis) {  //num_neighbors  //edit
			for (int j = 0; j < num_neighbors; j++) {
			  //mat_A0(j, 0) = laser_cloud_surf_from_map_->points[point_search_idx[j]].x;  //同下面两个
			  mat_A0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
			  mat_A0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
			  mat_A0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
			}
			mat_X0 = mat_A0.colPivHouseholderQr().solve(mat_B0);

			float pa = mat_X0(0, 0);
			float pb = mat_X0(1, 0);
			float pc = mat_X0(2, 0);
			float pd = 1;

			float ps = sqrt(pa * pa + pb * pb + pc * pc);
			pa /= ps;
			pb /= ps;
			pc /= ps;
			pd /= ps;

			// NOTE: plane as (x y z)*w+1 = 0

			bool planeValid = true;
			for (int j = 0; j < num_neighbors; j++) {
			  if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +  //laser_cloud_surf_from_map_  //edit
				  pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
				  pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > min_plane_dis) {  //min_plane_dis_ = 0.2 //edit  
				planeValid = false;
				break;
			  }
			}

			if (planeValid) {
				Eigen::Vector3d curr_point(point_ori.x, point_ori.y, point_ori.z); 
				Eigen::Vector3d norm(pa, pb, pc); 
				ceres::CostFunction *cost_function = LioLidarPlaneNormFactor::Create(curr_point, norm, double(pd)); 
				problem.AddResidualBlock(cost_function, loss_function, lioposeparam);
			}
		  }
		}
		ceres::Solver::Options options; 
        options.linear_solver_type = ceres::DENSE_QR; 
        options.max_num_iterations = 4; 
        options.minimizer_progress_to_stdout = false; 
        options.check_gradients = false; 
        options.gradient_check_relative_precision = 1e-4; 
        ceres::Solver::Summary summary; 
        ceres::Solve(options, &problem, &summary); 
        //std::cout << summary.BriefReport() << std::endl;
        transform_tobe_mapped_.rot.x() = lioposeparam[0];
        transform_tobe_mapped_.rot.y() = lioposeparam[1];
        transform_tobe_mapped_.rot.z() = lioposeparam[2];
        transform_tobe_mapped_.rot.w() = lioposeparam[3];
        transform_tobe_mapped_.pos.x() = lioposeparam[4];
        transform_tobe_mapped_.pos.y() = lioposeparam[5];
        transform_tobe_mapped_.pos.z() = lioposeparam[6];
	}
		  transformUpdate();

	}

    void MultiProcessMap() {
        convergence = 0;
		
        //if (laser_cloud_corner_from_map_->points.size() <= 10 || laser_cloud_surf_from_map_->points.size() <= 100) {
        if (laserCloudCornerFromMapDS->points.size() <= 10 || laserCloudSurfFromMapDS->points.size() <= 100) {  //已经是全局坐标了
            LOG(ERROR) << "skip due to insufficient points";
            return;
        }

        PointT point_sel, point_ori, coeff_, abs_coeff;

        std::vector<int> point_search_idx(5, 0);
        std::vector<float> point_search_sq_dis(5, 0);

        //pcl::KdTreeFLANN<PointT>::Ptr kdtree_corner_from_map(new pcl::KdTreeFLANN<PointT>());
        //pcl::KdTreeFLANN<PointT>::Ptr kdtree_surf_from_map(new pcl::KdTreeFLANN<PointT>());

        //kdtree_corner_from_map->setInputCloud(laser_cloud_corner_from_map_);
        //kdtree_surf_from_map->setInputCloud(laser_cloud_surf_from_map_);
        kdtreeCornerFromMap1->setInputCloud(laserCloudCornerFromMapDS);
        kdtreeSurfFromMap1->setInputCloud(laserCloudSurfFromMapDS);

        Eigen::Matrix<float, 5, 3> mat_A0;
        Eigen::Matrix<float, 5, 1> mat_B0;
        Eigen::Vector3f mat_X0;
        Eigen::Matrix3f mat_A1;
        Eigen::Matrix<float, 1, 3> mat_D1;
        Eigen::Matrix3f mat_V1;

        mat_A0.setZero();
        mat_B0.setConstant(-1);
        mat_X0.setZero();

        mat_A1.setZero();
        mat_D1.setZero();
        mat_V1.setZero();

        bool is_degenerate = false;
        matP_.setIdentity();

        //size_t laser_cloud_corner_stack_size = laser_cloud_corner_stack_downsampled_->points.size();
        //size_t laser_cloud_surf_stack_size = laser_cloud_surf_stack_downsampled_->points.size();
        //size_t laser_cloud_corner_stack_size = laserCloudCornerLastDS->points.size();  //当前帧角点
        //size_t laser_cloud_surf_stack_size = laserCloudSurfTotalLastDS->points.size();  //当前帧面点

        PointCloud laser_cloud_ori;
        PointCloud coeff_sel;

        PointCloud laser_cloud_ori_spc;
        score_point_coeff_.clear();
        
        TicToc tic_toc_;
        tic_toc_.Tic();  //edit
        for (int i = 0; i < frame_size; ++i) {
            LOG(INFO) << "before opt trans[" << i << "] = " << all_laser_transforms_[i].pos.transpose();
        }
        
	    size_t maximumiter = 2;
	    for (size_t iter_count = 0; iter_count < maximumiter; ++iter_count) {  //num_max_iterations_ = 10；  //edit 
            laser_cloud_ori.clear();
            coeff_sel.clear();

            laser_cloud_ori_spc.clear();

            ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
            ceres::Problem::Options problem_options;
            ceres::Problem problem(problem_options);

            for (int i = 0; i < frame_size; ++i) {
                ceres::LocalParameterization *local_parameterization = new AutoPoseLocalParameterization();
                problem.AddParameterBlock(para_pose_[i], 7, local_parameterization);
            }

            VectortoDouble();

            for(int k = 0; k < frame_size; ++k) {

                int coner_stack_size = corner_stack_[k]->size();
                int surf_stack_size = surf_stack_[k]->size();
                transform_tobe_mapped_ = all_laser_transforms_[k];

                LOG(INFO) << "corner_stack_[" << k << "].size = " << coner_stack_size;
                LOG(INFO) << "surf_stack_[" << k << "].size = " << surf_stack_size;

                //for (int i = 0; i < laser_cloud_corner_stack_size; ++i) {  //角点迭代
                for (int i = 0; i < coner_stack_size; ++i) {  //角点迭代
                    point_ori = corner_stack_[k]->points[i];
                    pointAssociateToMap(point_ori, point_sel, transform_tobe_mapped_);
                    //pointAssociateToMap(&pointOri, &pointSel);
                    // pointAssociateToMap(&point_ori, &point_sel);
                    //kdtree_corner_from_map->nearestKSearch(point_sel, 5, point_search_idx, point_search_sq_dis);
                    //kdtreeCornerFromMap1->nearestKSearch(point_sel, 5, point_search_idx, point_search_sq_dis);
                    kdtreeCornerFromMap1->nearestKSearch(point_sel, 5, pointSearchInd, pointSearchSqDis);

                    if (pointSearchSqDis[4] < min_match_sq_dis) {  //min_match_sq_dis_ = 1.0；  //edit
                //        Vector3Intl vc(0, 0, 0);
                        Eigen::Vector3f vc(0, 0, 0);

                        for (int j = 0; j < 5; j++) {
                //          vc += Vector3Intl(laser_cloud_corner_from_map_->points[point_search_idx[j]]);
                        //const PointT &point_sel_tmp = laser_cloud_corner_from_map_->points[point_search_idx[j]];
                        const PointT &point_sel_tmp = laserCloudCornerFromMapDS->points[pointSearchInd[j]];
                        vc.x() += point_sel_tmp.x;
                        vc.y() += point_sel_tmp.y;
                        vc.z() += point_sel_tmp.z;
                        }
                        vc /= 5.0;

                        Eigen::Matrix3f mat_a;
                        mat_a.setZero();

                        for (int j = 0; j < 5; j++) {
                //          Vector3Intl a = Vector3Intl(laser_cloud_corner_from_map_->points[point_search_idx[j]]) - vc;
                        //const PointT &point_sel_tmp = laser_cloud_corner_from_map_->points[point_search_idx[j]];
                            const PointT &point_sel_tmp = laserCloudCornerFromMapDS->points[pointSearchInd[j]];
                            Eigen::Vector3f a;
                            a.x() = point_sel_tmp.x - vc.x();
                            a.y() = point_sel_tmp.y - vc.y();
                            a.z() = point_sel_tmp.z - vc.z();

                            mat_a(0, 0) += a.x() * a.x();
                            mat_a(1, 0) += a.x() * a.y();
                            mat_a(2, 0) += a.x() * a.z();
                            mat_a(1, 1) += a.y() * a.y();
                            mat_a(2, 1) += a.y() * a.z();
                            mat_a(2, 2) += a.z() * a.z();
                        }
                        mat_A1 = mat_a / 5.0;

                        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(mat_A1);
                        mat_D1 = esolver.eigenvalues().real();
                        mat_V1 = esolver.eigenvectors().real();

                        if (mat_D1(0, 2) > 3 * mat_D1(0, 1)) {
                            float x0 = point_sel.x;
                            float y0 = point_sel.y;
                            float z0 = point_sel.z;
                            float x1 = vc.x() + 0.1 * mat_V1(0, 2);
                            float y1 = vc.y() + 0.1 * mat_V1(1, 2);
                            float z1 = vc.z() + 0.1 * mat_V1(2, 2);
                            float x2 = vc.x() - 0.1 * mat_V1(0, 2);
                            float y2 = vc.y() - 0.1 * mat_V1(1, 2);
                            float z2 = vc.z() - 0.1 * mat_V1(2, 2);

                            Eigen::Vector3d X0(point_ori.x, point_ori.y, point_ori.z);
                            Eigen::Vector3d X1(x1, y1, z1);
                            Eigen::Vector3d X2(x2, y2, z2);

                            ceres::CostFunction *cost_function = LioLidarEdgeFactorPose::Create(X0, X1, X2, 1.0); 
                            problem.AddResidualBlock(cost_function, loss_function, para_pose_[k]);
                        }
                    }
                }
                //for (int i = 0; i < laser_cloud_surf_stack_size; i++) {  //面点迭代
                for (int i = 0; i < surf_stack_size; i++) {  //面点迭代  //edit downsize laserCloudSurfTotalLastDSNum  //edit feature point
                    //point_ori = laser_cloud_surf_stack_downsampled_->points[i];
                    point_ori = surf_stack_[k]->points[i];  //edit downsize laserCloudSurfTotalLastDS   //edit feature point
                    pointAssociateToMap(point_ori, point_sel, transform_tobe_mapped_);
                    // pointAssociateToMap(&point_ori, &point_sel);

                    int num_neighbors = 5;
                    //kdtree_surf_from_map->nearestKSearch(point_sel, num_neighbors, point_search_idx, point_search_sq_dis);
                    //kdtreeSurfFromMap1->nearestKSearch(point_sel, num_neighbors, point_search_idx, point_search_sq_dis);
                    kdtreeSurfFromMap1->nearestKSearch(point_sel, num_neighbors, pointSearchInd, pointSearchSqDis);

                    if (pointSearchSqDis[num_neighbors - 1] < min_match_sq_dis) {  //num_neighbors  //edit
                        for (int j = 0; j < num_neighbors; j++) {
                        //mat_A0(j, 0) = laser_cloud_surf_from_map_->points[point_search_idx[j]].x;  //同下面两个
                        mat_A0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                        mat_A0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                        mat_A0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                        }
                        mat_X0 = mat_A0.colPivHouseholderQr().solve(mat_B0);

                        float pa = mat_X0(0, 0);
                        float pb = mat_X0(1, 0);
                        float pc = mat_X0(2, 0);
                        float pd = 1;

                        float ps = sqrt(pa * pa + pb * pb + pc * pc);
                        pa /= ps;
                        pb /= ps;
                        pc /= ps;
                        pd /= ps;

                        // NOTE: plane as (x y z)*w+1 = 0

                        bool planeValid = true;
                        for (int j = 0; j < num_neighbors; j++) {
                        if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +  //laser_cloud_surf_from_map_  //edit
                            pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                            pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > min_plane_dis) {  //min_plane_dis_ = 0.2 //edit  
                            planeValid = false;
                            break;
                        }
                        }

                        if (planeValid) {
                            Eigen::Vector3d curr_point(point_ori.x, point_ori.y, point_ori.z); 
                            Eigen::Vector3d norm(pa, pb, pc); 
                            ceres::CostFunction *cost_function = LioLidarPlaneNormFactor::Create(curr_point, norm, double(pd)); 
                            problem.AddResidualBlock(cost_function, loss_function, para_pose_[k]);
                        }
                    }
                }
            }
            ceres::Solver::Options options; 
            // options.linear_solver_type = ceres::DENSE_QR; 
            options.linear_solver_type = ceres::DENSE_SCHUR; 
            options.trust_region_strategy_type = ceres::DOGLEG;
            // options.max_num_iterations = 4; 
            options.minimizer_progress_to_stdout = false; 
            options.check_gradients = false; 
            options.gradient_check_relative_precision = 1e-4; 
            ceres::Solver::Summary summary; 
            ceres::Solve(options, &problem, &summary); 
            std::cout << "lio multi " << summary.BriefReport() << std::endl;
            LOG(INFO) <<"t_opt: " << tic_toc_.Toc() << " ms";
            DoubletoVector();
	    }

        for (int i = 0; i < frame_size; ++i) {
            LOG(INFO) << "after opt trans[" << i << "] = " << all_laser_transforms_[i].pos.transpose();
        }
    }

    void addLoopFactor()
    {
        if (loopIndexQueue.empty())
            return;

        for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
        {
            int indexFrom = loopIndexQueue[i].first;
            int indexTo = loopIndexQueue[i].second;
            gtsam::Pose3 poseBetween = loopPoseQueue[i];
            gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
            gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
        }

        loopIndexQueue.clear();
        loopPoseQueue.clear();
        loopNoiseQueue.clear();
        aLoopIsClosed = true;
    }

    bool saveFrame()
    {
        if (cloudKeyPoses3D->points.empty())
            return true;

        Eigen::Affine3f transStart = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->back());
        Eigen::Affine3f transFinal = transform_tobe_mapped_.transform();
        Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);

        if (abs(roll)  < surroundingkeyframeAddingAngleThreshold &&
            abs(pitch) < surroundingkeyframeAddingAngleThreshold && 
            abs(yaw)   < surroundingkeyframeAddingAngleThreshold &&
            sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
            return false;

        return true;
    }

    void saveKeyFramesAndFactor(bool update){

        if (saveFrame() == false)
            return;

        if (cloudKeyPoses3D->points.empty()){  //cloudKeyPoses3D->points.empty()
            gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3(transform_tobe_mapped_.rot.normalized().template cast<double>()),
                                                            Point3(transform_tobe_mapped_.pos.x(), transform_tobe_mapped_.pos.y(), transform_tobe_mapped_.pos.z())), priorNoise));
            initialEstimate.insert(0, Pose3(Rot3(transform_tobe_mapped_.rot.normalized().template cast<double>()),
                                                            Point3(transform_tobe_mapped_.pos.x(), transform_tobe_mapped_.pos.y(), transform_tobe_mapped_.pos.z())));
            transform_gtsam_last_ = transform_tobe_mapped_;
        }
        else{
            gtsam::Pose3 poseFrom = Pose3(Rot3(transform_gtsam_last_.rot.normalized().template cast<double>()),
                                                Point3(transform_gtsam_last_.pos.x(), transform_gtsam_last_.pos.y(), transform_gtsam_last_.pos.z()));
            gtsam::Pose3 poseTo   = Pose3(Rot3(transform_aft_mapped_.rot.normalized().template cast<double>()),
                                                Point3(transform_aft_mapped_.pos.x(), transform_aft_mapped_.pos.y(), transform_aft_mapped_.pos.z()));
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
            initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3(transform_aft_mapped_.rot.normalized().template cast<double>()),
                                                                        Point3(transform_aft_mapped_.pos.x(), transform_aft_mapped_.pos.y(), transform_aft_mapped_.pos.z())));
        }

        isam->update(gtSAMgraph, initialEstimate);
        isam->update();
        
        gtSAMgraph.resize(0);
		initialEstimate.clear();
        
        PointType thisPose3D;
        PointTypePose thisPose6D;
        if (update) {   //使用isam2的值作为transform_aft_mapped的值
            
            addLoopFactor();
            if (aLoopIsClosed == true)
            {
                isam->update();
                isam->update();
                isam->update();
                isam->update();
                isam->update();
            }
            Pose3 latestEstimate;

            isamCurrentEstimate = isam->calculateEstimate();
            latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);

            thisPose3D.x = latestEstimate.translation().x();
            thisPose3D.y = latestEstimate.translation().y();
            thisPose3D.z = latestEstimate.translation().z();
            thisPose3D.intensity = cloudKeyPoses3D->points.size();  //intensity就是次序
            cloudKeyPoses3D->push_back(thisPose3D);

        /*if(Num_rotation == 11){
        filter_rot_start = cloudKeyPoses3D->points.size();
    ROS_INFO("filter_rot_start: %d", filter_rot_start);
        }  //edit filter rotation start*/

            thisPose6D.x = thisPose3D.x;
            thisPose6D.y = thisPose3D.y;
            thisPose6D.z = thisPose3D.z;
            thisPose6D.intensity = thisPose3D.intensity;
            thisPose6D.qw = latestEstimate.rotation().toQuaternion().w(); 
            thisPose6D.qx = latestEstimate.rotation().toQuaternion().x();
            thisPose6D.qy = latestEstimate.rotation().toQuaternion().y();
            thisPose6D.qz = latestEstimate.rotation().toQuaternion().z();
            thisPose6D.time = timeLaserOdometry;
            cloudKeyPoses6D->push_back(thisPose6D);
            LOG(INFO) << "isamCurrentEstimate.size() = " << isamCurrentEstimate.size();
            LOG(INFO) << "cloud key pose 3D size = " << cloudKeyPoses3D->points.size();
            LOG(INFO) << "latestEstimate = " << latestEstimate.translation().transpose();
            if (cloudKeyPoses3D->points.size() > 1){
                transform_aft_mapped_.rot = latestEstimate.rotation().toQuaternion().template cast<float>(); //change num
                transform_aft_mapped_.pos.x() = latestEstimate.translation().x();
                transform_aft_mapped_.pos.y() = latestEstimate.translation().y();
                transform_aft_mapped_.pos.z() = latestEstimate.translation().z();
                
                transform_tobe_mapped_ = transform_aft_mapped_;
                transform_gtsam_last_ = transform_aft_mapped_;
            }
            LOG(INFO) << "after save gtsam beto = " << transform_tobe_mapped_.pos.transpose();
            LOG(INFO) << "after save gtsam after = " << transform_aft_mapped_.pos.transpose();
        } else {  //不使用isam2的值
            thisPose3D.x = transform_aft_mapped_.pos.x();
            thisPose3D.y = transform_aft_mapped_.pos.y();
            thisPose3D.z = transform_aft_mapped_.pos.z();
            thisPose3D.intensity = cloudKeyPoses3D->points.size();  //intensity就是次序
            cloudKeyPoses3D->push_back(thisPose3D);

        /*if(Num_rotation == 11){
        filter_rot_start = cloudKeyPoses3D->points.size();
    ROS_INFO("filter_rot_start: %d", filter_rot_start);
        }  //edit filter rotation start*/

            thisPose6D.x = thisPose3D.x;
            thisPose6D.y = thisPose3D.y;
            thisPose6D.z = thisPose3D.z;
            thisPose6D.intensity = thisPose3D.intensity;
            thisPose6D.qw = transform_aft_mapped_.rot.w(); 
            thisPose6D.qx = transform_aft_mapped_.rot.x();
            thisPose6D.qy = transform_aft_mapped_.rot.y();
            thisPose6D.qz = transform_aft_mapped_.rot.z();
            thisPose6D.time = timeLaserOdometry;
            cloudKeyPoses6D->push_back(thisPose6D);
            LOG(INFO) << "cloud key pose 3D size = " << cloudKeyPoses3D->points.size();
            if (cloudKeyPoses3D->points.size() > 1){                
                transform_tobe_mapped_ = transform_aft_mapped_;
                transform_gtsam_last_ = transform_aft_mapped_;
            }
            LOG(INFO) << "no update after save gtsam beto = " << transform_tobe_mapped_.pos.transpose();
            LOG(INFO) << "no update after save gtsam after = " << transform_aft_mapped_.pos.transpose();
        }
        

        pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr thisOutlierKeyFrame(new pcl::PointCloud<PointType>());

        pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);  //edit downsize DS
        pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);  //edit downsize DS
        //pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);  //edit downsize DS  //edit opt memory

	// for (int i = 0; i < 1; i++) //edit opt memory
	// {
	// 	thisOutlierKeyFrame->push_back(laserCloudOutlierLastDS->points[i]);  
	// }

        //pcl::copyPointCloud(*laserCloudCornerLastDSformap,  *thisCornerKeyFrame);  //edit downsize 2nd
        //pcl::copyPointCloud(*laserCloudSurfLastDSformap,    *thisSurfKeyFrame);  //edit downsize 2nd
        //pcl::copyPointCloud(*laserCloudOutlierLastDSformap, *thisOutlierKeyFrame);  //edit downsize 2nd

        cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
        surfCloudKeyFrames.push_back(thisSurfKeyFrame);
        outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);

        int cur_key = cloudKeyPoses3D->size() - 1;

        if (savePCD) {
            pcl::PCDWriter pcd_writer;
            std::string pcd_dir = savePCDDirectory+ "origin_cloud/" + std::to_string(cur_key) + ".pcd";
            pcd_writer.writeBinary(pcd_dir, *laserCloudOutlierLastIntensity);
        }

        static int count_save = 0;
        int caculate_num = 2;
        count_save++;
        if (count_save % caculate_num == 0) {
            Eigen::Matrix4f rotateToWorld, rotateiSubOneToi;
            rotateToWorld.setIdentity();
            rotateToWorld.block<3, 3>(0, 0) = transform_tobe_mapped_.rot.toRotationMatrix();

            // 将cur-1的点云转换到cur，再将相加后的点云旋转到世界系
            Transform trans_curSubOne, trans_i_iSubOne;
            trans_curSubOne.rot = Eigen::Quaternionf(cloudKeyPoses6D->points[cur_key - 1].qw,
                                                     cloudKeyPoses6D->points[cur_key - 1].qx,
                                                     cloudKeyPoses6D->points[cur_key - 1].qy,
                                                     cloudKeyPoses6D->points[cur_key - 1].qz);

            trans_curSubOne.pos = Eigen::Vector3f(cloudKeyPoses6D->points[cur_key - 1].x,
                                                  cloudKeyPoses6D->points[cur_key - 1].y,
                                                  cloudKeyPoses6D->points[cur_key - 1].z);

            trans_i_iSubOne = transform_tobe_mapped_.inverse() * trans_curSubOne;

            pcl::transformPointCloud(*fullCloudAlignWorld, *fullCloudAlignWorld, trans_i_iSubOne.transform());

            *fullCloudAlignWorld += *laserCloudOutlierLastIntensity;

            // pcl::PointCloud<PointType> alignCloud;
            pcl::transformPointCloud(*fullCloudAlignWorld, *fullCloudAlignWorld, rotateToWorld);

            if (savePCD) {
                saveFullCloudAndKey(fullCloudAlignWorld, cur_key);
            }

            cv::Mat1b li1 = LidarIris::GetIris(*fullCloudAlignWorld, irisConfig);
            float mindis = 1000;
            int loopID = -1;
            lidarIris->UpdateFrame(li1, cur_key, &mindis, &loopID);
            PointType cur_point;
            cur_point.x = thisPose3D.x;
            cur_point.y = thisPose3D.y;
            cur_point.z = thisPose3D.z;
            cur_point.intensity = featurePoses->points.size();
            featurePoses->points.push_back(cur_point);

        } else {
            fullCloudAlignWorld->clear();
            *fullCloudAlignWorld = *laserCloudOutlierLastIntensity;
        }
        
        // irismanager->makeAndSaveIrisAndKey<PointType>(laserCloudOutlierLastIntensity);

        updatePath(thisPose6D);

		if(is_record_map)  //edit loop
		{
			pcl::PointXYZI zeropoint;  //反射强度修改  //反射强度精简
			zeropoint.x = 0;
			zeropoint.y = 0;
			zeropoint.z = 0;
			zeropoint.intensity = 0;  //反射强度修改  //反射强度精简
			
//ROS_INFO("is_record_map: %d", is_record_map);

			if(aLoopIsClosed)
			{
				zeroPCount = 0;
			}

			int thisOutliersize;
			if(zeroPCount < 10 || !flag_rotation)  //edit 1 zero point
			{
			thisOutliersize = 1;
			save_pointsize_for_map(thisOutliersize);
			}
			else
			{
			thisOutliersize = laserCloudOutlierLastIntensity->points.size();
			save_pointsize_for_map(thisOutliersize);
			}
			
		fout_point.open(point_bfile.c_str(), ios::app | ios ::binary | ios::out);
		if(!fout_point.is_open())
			cout << "点云文件打开失败！" << endl;
			
			if(zeroPCount < 10 || !flag_rotation)
			{
				for (int i = 0; i < 1; i++)
				{
					save_point_for_map(zeropoint);
				}
				zeroPCount++;
			}
			else
			{
				for (int i = 0; i < laserCloudOutlierLastIntensity->points.size(); i++)
				{
					float dis2orig2 = laserCloudOutlierLastIntensity->points[i].x*laserCloudOutlierLastIntensity->points[i].x + laserCloudOutlierLastIntensity->points[i].y*laserCloudOutlierLastIntensity->points[i].y + laserCloudOutlierLastIntensity->points[i].z*laserCloudOutlierLastIntensity->points[i].z;  //edit point filter distance
					if (dis2orig2 > 2.25){
					save_point_for_map(laserCloudOutlierLastIntensity->points[i]);
					}else{
					save_point_for_map(zeropoint);
					}
				}
			}

		fout_point.close();

			Num_Points++;
			ROS_INFO("Num_Points: %d", Num_Points);
		}
		
    }

    void correctPoses(){

        if (cloudKeyPoses3D->points.empty())
            return;

    	if (aLoopIsClosed == true){
            recentCornerCloudKeyFrames. clear();
            recentSurfCloudKeyFrames.   clear();
            recentOutlierCloudKeyFrames.clear();
            surroundingExistingKeyPosesID.clear();
            surroundingCornerCloudKeyFrames.clear();
            surroundingSurfCloudKeyFrames.clear();
            surroundingOutlierCloudKeyFrames.clear();
            LOG(INFO) << "loop";
            LOG(INFO) << "pose before loop";
            LOG(INFO) << "pose num" << cloudKeyPoses3D->size() << std::endl;
            
            // clear map cache
            laserCloudMapContainer.clear();
            // clear path
            globalPath.poses.clear();

            int numPoses = isamCurrentEstimate.size();
			for (int i = 0; i < numPoses; ++i)
			{
				cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();//change num
                cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
                cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();

                cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
                cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
                cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
                cloudKeyPoses6D->points[i].qw = isamCurrentEstimate.at<Pose3>(i).rotation().toQuaternion().w();//change num
                cloudKeyPoses6D->points[i].qx = isamCurrentEstimate.at<Pose3>(i).rotation().toQuaternion().x();
                cloudKeyPoses6D->points[i].qy = isamCurrentEstimate.at<Pose3>(i).rotation().toQuaternion().y();
                cloudKeyPoses6D->points[i].qz = isamCurrentEstimate.at<Pose3>(i).rotation().toQuaternion().z();

                updatePath(cloudKeyPoses6D->points[i]);
			}
			
			int loop = 1;
			ROS_INFO("loop: %d", loop);

	    	aLoopIsClosed = false;
	    }
		
		if(is_record_pose)  //edit loop
		{
			
			for (int i = 0; i < cloudKeyPoses3D->points.size(); i++)
			{
				save_Poses6D_for_map(cloudKeyPoses6D->points[i]);
				Num_Poses++;
			}		

			ROS_INFO("Num_Poses: %d", Num_Poses);
			
			clearCloud();    //edit loop ????
			
			yes_record_pose = true;
		}
		
		
    }

    void clearCloud(){
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear();  
        laserCloudCornerFromMapDS->clear();
        laserCloudSurfFromMapDS->clear();   
    }

    void saveFullCloudAndKey(pcl::PointCloud<PointType>::Ptr &full, int key) {
        pcl::PCDWriter pcd_writer;
        std::string pcd_dir = savePCDDirectory + std::to_string(key) + ".pcd";
        pcd_writer.writeBinary(pcd_dir, *full);
        fullCloudHash[key] = pcd_dir;
    }

    void saveAllKeyPoses6D() {
        std::ofstream outfile;
        std::string dir = savePCDDirectory + "keyposes.txt";
        outfile.open(dir);
        for (int i = 0; i < cloudKeyPoses6D->points.size(); ++i) {
            outfile << ros::Time().fromSec(cloudKeyPoses6D->points[i].time) << " "
                    << cloudKeyPoses6D->points[i].x << " " 
                    << cloudKeyPoses6D->points[i].y << " "
                    << cloudKeyPoses6D->points[i].z << " "
                    << cloudKeyPoses6D->points[i].qx << " "
                    << cloudKeyPoses6D->points[i].qy << " "
                    << cloudKeyPoses6D->points[i].qz << " "
                    << cloudKeyPoses6D->points[i].qw << std::endl;
        }
        outfile.close();
    }

    void run(){
        // if(newLaserCloudCornerLast || newLaserCloudSurfLast || newLaserCloudOutlierLast || newLaserOdometry) {
        //     LOG(INFO) << "newLaserCloudCornerLast  = " << newLaserCloudCornerLast;
        //     LOG(INFO) << "newLaserCloudSurfLast    = " << newLaserCloudSurfLast;
        //     LOG(INFO) << "newLaserCloudOutlierLast = " << newLaserCloudOutlierLast;
        //     LOG(INFO) << "newLaserOdometry         = " << newLaserOdometry;
        //     LOG(INFO) << "timeLaserCloudCornerLast  - timeLaserOdometry = " << timeLaserCloudCornerLast  - timeLaserOdometry;
        //     LOG(INFO) << "timeLaserCloudSurfLast    - timeLaserOdometry = " << timeLaserCloudSurfLast    - timeLaserOdometry;
        //     LOG(INFO) << "timeLaserCloudOutlierLast - timeLaserOdometry = " << timeLaserCloudOutlierLast - timeLaserOdometry;
        // }
        
        // if (newLaserCloudCornerLast  && std::abs(timeLaserCloudCornerLast  - timeLaserOdometry) < 0.005 &&
        //     newLaserCloudSurfLast    && std::abs(timeLaserCloudSurfLast    - timeLaserOdometry) < 0.005 &&
        //     newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
        //     newLaserOdometry)
        // {
        while (true) {
            std::unique_lock<std::mutex> buf_lk(buf_mutex_);
            con_.wait(buf_lk, [&] {
                return (GetMapMeasurements());
            });  //得到缓冲区中的点云
            buf_lk.unlock();    
            int data_size = compact_transform_sums_.size();
            LOG(INFO) << "GetMapMeasurements size = " << data_size;
            if(compact_transform_sums_.size() != compact_corner_clouds_.size()) {
                LOG(INFO) << "compact_transform_sums_.size() != compact_corner_clouds_.size()";
                continue;
            }
            if(compact_corner_clouds_.size() != compact_surf_clouds_.size()) {
                LOG(INFO) << "compact_corner_clouds_.size() != compact_surf_clouds_.size()";
                continue;
            }
            if(compact_surf_clouds_.size() != compact_full_clouds_.size()) {
                LOG(INFO) << "compact_surf_clouds_.size() != compact_full_clouds_.size()";
                continue;
            }
            //对得到的缓冲区中的点云进行处理
            for(int data_num = 0; data_num < data_size; ++data_num) {
                transform_sum_ = compact_transform_sums_.front();
                compact_transform_sums_.pop();

                laserCloudCornerLast->clear();
                *(laserCloudCornerLast) = *(compact_corner_clouds_.front());
                compact_corner_clouds_.pop();

                laserCloudSurfLast->clear();
                *(laserCloudSurfLast) = *(compact_surf_clouds_.front());
                compact_surf_clouds_.pop();

                laserCloudOutlierLastIntensity->clear();
                *(laserCloudOutlierLastIntensity) = *(compact_full_clouds_.front());
                compact_full_clouds_.pop();
                
                timeLaserOdometry = compact_timestamps_.front().toSec();
                compact_timestamps_.pop();
                TicToc tprocess;
                tprocess.Tic();
                if (!system_init_) {
                    system_init_ = true;
                    transform_bef_mapped_ = transform_sum_;
                    transform_tobe_mapped_ = transform_sum_;
                    transform_aft_mapped_ = transform_tobe_mapped_;
                }
                // newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;

                std::lock_guard<std::mutex> lock(mtx);

                double timeinter = timeLaserOdometry - timeLastProcessing;
                ROS_INFO("==========timeinter: %f", timeinter);
                LOG(INFO) << "==========timeinter: " << timeinter;
                if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {  //mappingProcessInterval

                    timeLastProcessing = timeLaserOdometry;

                    ROS_INFO("mapNum: %d", mapNum);
                    LOG(INFO) << "mapNum: " << mapNum;
                
                if (flag_normal_) {     //单帧情况
                    if(!flag_4Dconstrain_/* && mapNum % 3 != 0*/)
                    {
                        transformAssociateToMap();
                        LOG(INFO) << "ONE FRAME NO constrain";
                    }
                    else
                    {
                        Transform4DAssociateToMap();  //edit 
                        LOG(INFO) << "ONE FRAME constrain";
                    }
                } else {     //多帧情况
                    if(flag_4Dconstrain_) {
                        LOG(INFO) << "MULTI FRAMEs constrain";
                        MultiTransform4DAssociateToMap();  //使用旋转约束
                    } else {
                        LOG(INFO) << "MULTI FRAMEs NO constrain";
                        MultitransformAssociateToMap();    //不用旋转约束
                    }
                    // 累积ｎ帧点云
                    laserCloudCornerLastDS->clear();
                    downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
                    downSizeFilterCorner.filter(*laserCloudCornerLastDS);
                    corner_stack_.push(boost::make_shared<PointCloud>(*laserCloudCornerLastDS));

                    laserCloudSurfLastDS->clear();
                    downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
                    downSizeFilterSurf.filter(*laserCloudSurfLastDS);
                    surf_stack_.push(boost::make_shared<PointCloud>(*laserCloudSurfLastDS));

                    full_stack_.push(boost::make_shared<PointCloud>(*laserCloudOutlierLastIntensity));

                    all_laser_transforms_.push(transform_tobe_mapped_);  //记录预估位姿
                    transform_sums_.push(transform_sum_);  //记录estimator发来的位姿
                    laser_odom_times_.push(timeLaserOdometry);  //记录每帧点云的时间戳

                    LOG(INFO) << "come a frame !!!!!";
                    LOG(INFO) << "corner size = " << corner_stack_.size();
                    LOG(INFO) << "surf size = " << surf_stack_.size();
                    LOG(INFO) << "full size = " << full_stack_.size();
                    LOG(INFO) << "laser_odom_times_ size = " << laser_odom_times_.size();
                    LOG(INFO) << "transform size = " << all_laser_transforms_.size();
                    LOG(INFO) << "transform_sums_ size = " << transform_sums_.size();
                }

                if(!flag_normal_ && all_laser_transforms_.size() != frame_size) {
                    LOG(INFO) << "frame not enough, return";
                    mapNum++;
                    continue;   //点云帧数不够时,直接return,直到帧数到达指定数目
                } else {
                    extractSurroundingKeyFrames();
                    
                    if (flag_normal_) {    //单帧优化                
                        downsampleCurrentScan();

                        if(flag_4Dconstrain_) {
                            LOG(INFO) << "ONE FRAMEs opt constrain"; 
                            ProcessMap();  //旋转约束
                        } else {  
                            LOG(INFO) << "ONE FRAMEs opt NO constrain";                   
                            scan2MapOptimization();  //非旋转约束
                        }

                        saveKeyFramesAndFactor(true);

                        correctPoses();

                        publishTF();  //odometry

                        publishKeyPosesAndFrames();  //key pose & recent clouds(两次降采样的全部面点)

                        clearCloud();

                        //将GTSAM优化后的transformtobemapped赋给transform_last_
                        transform_last_ = transform_tobe_mapped_;
                        LOG(INFO) << "one transform_last = " << transform_last_.pos.transpose();

                    } else {    //多帧优化
                        if(flag_4Dconstrain_) {
                            LOG(INFO) << "MULTI FRAMEs opt constrain"; 
                            MultiProcessMap();  //旋转约束
                        } else {  
                            LOG(INFO) << "MULTI FRAMEs opt NO constrain";                   
                            Multiscan2MapOptimization();  //非旋转约束
                        }

                        pcl::PointCloud<PointType>::Ptr accumulateCloudFull = pcl::make_shared<pcl::PointCloud<PointType>>();
                        pcl::PointCloud<PointType>::Ptr accumulateCloudCorner = pcl::make_shared<pcl::PointCloud<PointType>>();
                        pcl::PointCloud<PointType>::Ptr accumulateCloudSurf = pcl::make_shared<pcl::PointCloud<PointType>>();
                        for(int i = 0; i < frame_size - 1; ++i) {
                            auto transform_itoLast = all_laser_transforms_.last().inverse() * all_laser_transforms_[i];
                            *accumulateCloudFull += *transformPointCloud(full_stack_[i], transform_itoLast);
                            *accumulateCloudCorner += *transformPointCloud(corner_stack_[i], transform_itoLast);
                            *accumulateCloudSurf += *transformPointCloud(surf_stack_[i], transform_itoLast);
                        }
                        *accumulateCloudFull += *full_stack_.last();
                        *accumulateCloudCorner += *corner_stack_.last();
                        *accumulateCloudSurf += *surf_stack_.last();

                        {
                            transform_tobe_mapped_ = all_laser_transforms_.last();
                            transform_aft_mapped_ = transform_tobe_mapped_;

                            laserCloudCornerLastDS->clear();
                            *laserCloudCornerLastDS = *(accumulateCloudCorner);

                            laserCloudSurfLastDS->clear();
                            *laserCloudSurfLastDS = *(accumulateCloudSurf);

                            laserCloudOutlierLastDS->clear();
                            downSizeFilterOutlier.setInputCloud(accumulateCloudFull);
                            downSizeFilterOutlier.filter(*laserCloudOutlierLastDS);

                            laserCloudOutlierLastIntensity->clear();
                            *laserCloudOutlierLastIntensity = *(accumulateCloudFull);
                            timeLaserOdometry = laser_odom_times_.last();
                            LOG(INFO) << "save key frame ";
                            LOG(INFO) << "7 to 6 before save gtsam tobe = " << transform_tobe_mapped_.pos.transpose();
                            LOG(INFO) << "7 to 6 before save gtsam after = " << transform_aft_mapped_.pos.transpose();
                            
                            bool update;
                            // if (i == (frame_size - 1)) {    //最后一帧用isam2的值更新tobemapped位姿,其他帧不用isam2的更新值
                            //     update = true;                                  
                            // } else {
                            //     update = false; 
                            // }
                            update = true;
                            // isam2因子添加的是增量位姿，所以对于3帧点云，要添加点面-点线优化后的位姿差。
                            saveKeyFramesAndFactor(update);

                            publishTF();  //odometry
                                              
                        }


                        // for(int i = 0; i < frame_size; ++i) {
                        //     transform_tobe_mapped_ = all_laser_transforms_[i];
                        //     transform_aft_mapped_ = transform_tobe_mapped_;

                        //     laserCloudCornerLastDS->clear();
                        //     *laserCloudCornerLastDS = *(corner_stack_[i]);

                        //     laserCloudSurfLastDS->clear();
                        //     *laserCloudSurfLastDS = *(surf_stack_[i]);

                        //     laserCloudOutlierLastDS->clear();
                        //     downSizeFilterOutlier.setInputCloud(full_stack_[i]);
                        //     downSizeFilterOutlier.filter(*laserCloudOutlierLastDS);

                        //     laserCloudOutlierLastIntensity->clear();
                        //     *laserCloudOutlierLastIntensity = *(full_stack_[i]);
                        //     timeLaserOdometry = laser_odom_times_[i];
                        //     LOG(INFO) << "save key frame " << i;
                        //     LOG(INFO) << "7 to 6 before save gtsam tobe = " << transform_tobe_mapped_.pos.transpose();
                        //     LOG(INFO) << "7 to 6 before save gtsam after = " << transform_aft_mapped_.pos.transpose();
                            
                        //     bool update;
                        //     if (i == (frame_size - 1)) {    //最后一帧用isam2的值更新tobemapped位姿,其他帧不用isam2的更新值
                        //         update = true;                                  
                        //     } else {
                        //         update = false; 
                        //     }
                        //     // isam2因子添加的是增量位姿，所以对于3帧点云，要添加点面-点线优化后的位姿差。
                        //     saveKeyFramesAndFactor(update);

                        //     publishTF();  //odometry                                                
                        // }


                        // PublishCloudMsg(pubLaserCloudmap, *accumulateCloudFull, ros::Time().fromSec(timeLaserOdometry), "world");
                        correctPoses(); //要在三帧中最后一帧保存完毕之后，再对cloudkeypose6D进行更新，如果放到循环里面，就会
                                        //造成第一帧的时候就会对cloudkeypose6D更新，而第二和三帧就不会更新
                                        //correctPoses函数里有保存位姿到文件的功能，可以不必放到循环里，放到循环外也行，
                                        // 也就是说直接把三帧位姿写到文件里

                        publishKeyPosesAndFrames();  //key pose & recent clouds(两次降采样的全部面点)

                        clearCloud();
                        //将GTSAM优化后的transformtobemapped赋给transform_last_
                        transform_last_ = transform_tobe_mapped_;
                        LOG(INFO) << "multi transform_last = " << transform_last_.pos.transpose();
                        LOG(INFO) << "multi tobemapped = " << transform_tobe_mapped_.pos.transpose();
                        transform_bef_mapped_ = transform_sums_.last(); 

                        // 清空点云以及位姿,为下一批点云做准备
                        for(int i = 0; i < frame_size; ++i) {
                            all_laser_transforms_.remove_front();
                            transform_sums_.remove_front();
                            corner_stack_.remove_front();
                            surf_stack_.remove_front();
                            full_stack_.remove_front();
                            laser_odom_times_.remove_front();
                        }
                    }

                    // 单帧的帧数要远小于100（约束与非约束切换点的帧数）或者远大于100，不能在100附近，容易出现逻辑错误
                    // 单帧情况主要是为了给出初始地图，所以单帧数目可以小一些，比如15
                    mapNum++;
                    // if (flag_4Dconstrain_) {
                    //     //帧数大于100且完成三帧优化之后，开始使用非旋转约束
                    //     if (mapNum > 100) {
                    //         flag_4Dconstrain_ = false;
                    //     }
                    // }
                    // 前skip_frames帧不使用多帧优化
                    if(flag_normal_) {
                        if(mapNum > skip_frames) {
                            flag_normal_ = false;
                        }
                    }
                    
                }                    
                }
                LOG(INFO) << "MO.run() time = " << tprocess.Toc();
            }
        }
    }
};
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "mapping");

    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");

    lio::mapOptimization MO;

    std::thread loopthread(&lio::mapOptimization::loopClosureThread, &MO);  //edit  
    std::thread visualizeMapThread(&lio::mapOptimization::visualizeGlobalMapThread, &MO);  //edit

    std::thread MO_run(&lio::mapOptimization::run, &MO);

    ros::Rate rate(100);
    while (ros::ok())
    {
        ros::spinOnce();

        // MO.run();

        rate.sleep();
    }

    if (MO.savePCD) {
        MO.saveAllKeyPoses6D();
    }

    loopthread.join();
    visualizeMapThread.join();
    MO_run.join();

    return 0;
}


